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ABSTRACT 


Millions  of  acres  of  the  US  formerly  used  defense  sites  (FUDS)  are  contaminated 
with  unexploded  ordnance  (UXO)  as  a  result  of  past  military  use.  The  process  of 
returning  the  land  to  the  civilian  sector  is  sensitive,  intensive,  and  costly  (e.g.,  millions  of 
dollars,  and  the  loss  of  human  life).  Hence  “clearing”  (i.e.,  site  remediation,  range 
clearance,  and  explosive  ordnance  disposal)  UXO’s  from  FUDS  is  a  complex  problem. 
Existing  clearing  methods  are  inaccurate,  dangerous,  and  labor  intensive.  This  thesis 
shows  that  through  robotics  technology  (e.g.,  “Shepherd”  rotary  vehicle  with  three 
degrees  of  freedom)  and  the  use  of  advanced  computer  technology  it  is  possible  to  make 
clearing  tasks  safer,  more  cost-effective,  and  more  efficient.  An  over  arching  hardware 
and  software  architecture  was  developed  for  Shepherd  (including  a  self-contained  on¬ 
board  computer  system).  The  software  system  was  developed  for  timer  control,  motion 
control,  user  interface,  and  an  operating  kernel.  The  hardware  and  software  organization, 
structure,  and  interaction  provide  the  framework  for  real-time  control.  This  research 
included  the  use  of  encoders,  digital  boards,  and  a  counter  board;  required  the  handling  of 
interrupts,  electric  motor  manipulation  by  servomotor  controllers,  and  communication 
using  RS232  and  VMEbus  technology.  The  kinematics  algorithms  and  a  real-time 
operating  kernel  were  implemented  using  the  C  language.  “Shepherd”  research  has  laid 
the  foundation  for  the  flexible,  robust,  and  precise  motion  needed  for  UXO  clearing. 
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I.  INTRODUCTION 


A.  MOTIVATION  AND  BACKGROUND 

Land  mines  are  an  inexpensive  and  effective  defensive  means  in  wars.  The 
problem  with  land  mines  is  that  they  remain  to  be  a  threat  when  wars  are  over. 
International  efforts  are  being  made  to  ensure  that  land  mines  deployed  in  the  future  are 
equipped  with  a  time-out  device,  and  mine  locations  are  properly  recorded.  While  such  a 
treaty  may  provide  relief  in  the  future,  millions  of  land  mines  were  planted  all  over  the 
world  as  a  result  of  wars  and  regional  conflicts  in  the  past. 

There  are  about  110  million  land  mines  scattered  around  the  world  in  more  than 
60  countries  —  most  of  them  in  the  Third  World  [Ref.  1,  2,  3,  4,  and  5].  These  land 
mines  kill  about  10,000  and  injure  another  20,000  people  (many  of  them  are  children) 
every  year.  Moreover,  there  are  millions  of  acres  of  the  US  formerly  used  defense  sites 
(FUDS)  that  are  contaminated  with  unexploded  ordnance  (UXO)  as  a  result  of  military 
testing  and  training  in  the  past  [Ref.  6],  The  contaminated  land  must  be  cleared  inch  by 
inch  before  transferring  to  civilian  use.  The  difficulties  of  these  clearing  missions  are  in 
the  variety  of  the  objects  to  be  identified  and  the  diversity  of  the  environments  that  are 
contaminated. 

B.  OBJECTIVES 

As  the  military  continues  to  downsize,  the  process  of  turning  the  land  over  to  the 
civilian  sector  is  sensitive,  intensive,  and  costly.  The  aforementioned  costs  are  both 
monetary  and  in  some  instance  the  loss  of  human  life.  One  of  the  most  complex 
problems  is  the  clearing  of  UXO’s  from  the  FUDS. 

The  Department  of  Defense  (DOD)  has  recently  approved  two  organizational 
structures  to  confront  the  challenge  of  UXO  remediation  and  wide-area  de-mining.  The 
objective  of  the  First  committee  is  to  develop  fully  coordinated  requirements  driven 
research  and  development  program  for  countermine,  de-mining,  site  remediation,  range 
clearance,  and  explosive  ordnance  disposal.  Within  the  first  committee  there  is  a  specific 
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group  focused  on  detection  technology.  The  second  committee  will  focus  on  current 
technologies  and  ways  to  improve  in  the  future.  One  of  the  phases  will  examine  current 
UXO  remediation,  active  range  UXO  clearance  and  explosive  ordnance  disposal  efforts. 
Hence,  the  UXO  problem  is  serious  and  a  highly  visible  issue  within  the  DOD.  The 
current  approach  to  mine  and  UXO  clearing  is  dangerous  and  labor  intensive  [Ref.  6].  In 
a  typical  UXO  clearing  scenario,  Explosive  Ordnance  Disposal  (EOD)  technicians  walk 
slowly  and  carefully  over  a  contaminated  field  in  an  attempt  to  identify  the  presence  of 
UXO’s  that  may  be  fully  buried,  half  buried,  or  totally  on  the  surface.  UXO’s  found  on 
the  surface  are  visually  examined  to  determine  their  types  and  fuse  mechanisms.  If  fuse 
mechanisms  are  recognized  and  the  condition  of  the  UXO  permits  (e.g.,  not  rusted, 
decayed,  or  encased  in  soil),  an  effort  is  made  to  defuse  UXO’s.  UXO’s  that  cannot  be 
defused  are  gathered  at  a  safe  location  and  are  destroyed  using  shaped  charges. 
Transporting  any  live  UXO  is  extremely  dangerous  and  the  motion  of  the  transport 
vehicle  must  be  gentle.  Moreover,  buried  UXO’s  must  be  unearthed  first.  Therefore, 
UXO  clearing  consists  of  detection,  identification,  defusing,  excavating,  transporting,  and 
disposal.  Through  robotics  and  the  use  of  advanced  technology  it  should  be  possible  to 
make  UXO  clearing  tasks  safer,  cost-effective,  and  more  efficient. 

At  the  Naval  Postgraduate  School  a  team  has  been  put  together  to  develop  a 
semi  autonomous  vehicle  or  robot,  which  will  survey  possible  contaminated  areas  for 
UXO’s.  The  “rotary”  class  vehicle  by  its  very  design  is  capable  of  independent  driving 
and  steering  with  each  wheel.  Hence,  rotary  vehicles  are  capable  of  stronger  torque  and 
traction  than  most  other  vehicles  on  rugged  terrains.  Rotary  vehicles  with  two  wheels  (on 
a  very  smooth  surface)  have  been  shown  to  possess  the  aforementioned  enhanced  torque 
and  traction  -  with  an  increase  in  the  number  of  wheels  on  the  vehicle  the  capabilities  are 
significantly  improved.  The  vehicle  needs  to  be  highly  mobile  and  capable  of  producing 
very  fine  motion  to  negotiate  and  search  contaminated  sites.  To  fulfill  the  requirement  of 
the  special  wheel  architecture,  the  semiautonomous  vehicle,  called  Shepherd  (a  rotary 
vehicle),  is  presently  under  development.  Shepherd  possesses  stronger  torque  and 
traction  on  rugged  terrain,  because  of  its  special  architecture  (i.e.,  useful  for  the 
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mine/UXO.  Mission).  The  name  “Shepherd”  was  given  to  the  vehicle  because  of  the 
protective  function  of  the  “shepherd”  in  many  ancient  cultures — hopefully  this  Shepherd 
will  also  save  lives.  Shepherd  (Figure  1.1)  is  a  four-wheeled  vehicle  with  independent 
steering  and  driving  capabilities.  The  four-wheel  independent  driving  and  steering 
capability  provides  Shepherd  a  high  level  of  mobility  and  preciseness  in  motion  control 
[Ref.  7]. 


Figure  1.1:  A  “rotary”  vehicle:  Shepherd. 


The  fundamental  objective  of  this  thesis  research  project  is  to  assist  in  the 
construction  of  a  user-friendly  and  high-precision  semiautonomous  robotics  tool  to  help 
the  unexploded  ordnance  (UXO)  and  mine  clearing  mission. 
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This  thesis  will  examine  the  following  research  areas: 

•  What  kinematics  algorithms  must  be  developed  to  support  a  vehicle  with  three 
degrees  of  freedom  of  motion?  The  aforementioned  algorithms  must  support 
highly  flexible,  controlled,  and  precise  motion. 

•  What  types  of  controls  are  required  to  ensure  the  optimal  mix  of  driving  and 
steering  resources?  Moreover,  what  must  be  done  to  ensure  that  all  the 
resources  complement? 

•  How  can  the  knowledge  gained  in  the  aforementioned  research  areas  be  used 
to  develop  searching  motion? 

•  How  should  the  hardware  and  software  systems  be  implemented  to  support 
the  aforementioned  goals? 

•  How  can  human  operators  remotely  control  the  vehicle  through  an  appropriate 
interface? 

C.  ORGANIZATION 

This  chapter  provides  a  general  overview  of  traditional  and  current  techniques  for 
identifying  unexploded  ordnance.  Chapter  II  provides  the  System  Overview  and 
illustrates  the  concept  of  motion  control.  Chapter  HI  describes  the  Shepherd  Mobile 
Platform  and  the  On-Board  Computer  System.  Chapter  IV  presents  the  Shepherd 
Software  Description  and  places  great  emphasis  on  the  Shepherd  Real-time  Kernel 
(SRK).  In  Chapter  V  the  results  of  experiments  and  testing  motion  control  are  presented. 
Chapter  VI  explains  the  current  motion  modes  and  Chapter  VII  summarizes  the  thesis. 
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II.  SHEPHERD  SYSTEM  DESIGN 


A.  SYSTEM  OVERVIEW 

In  consideration  of  aiding  the  UXO  task,  what  type  of  vehicle  should  be 
developed?  This  vehicle  will  face  difficulties  of  clearing  missions  in  the  variety  of  the 
objects  to  be  identified  and  the  diversity  of  the  environments  that  are  contaminated.  This 
vehicle  has  to  have  precise  and  smooth  motion,  display  motion  flexibility,  and  contain 
robust  motion  in  varied  environments  such  as  soft  soil  and  rough  terrain. 

The  vehicle  must  be  capable  of  precise  and  smooth  motion  while  searching  for 
UXO’s.  The  very  nature  of  trying  to  locate  UXO’s  should  be  meticulous  and  cautious. 
Haphazzard  and  jerky  motions  could  contribute  to  lost  of  the  vehicle  due  to  unwanted 
detonations.  Also,  motion  flexibility  is  absolutely  necessary.  This  will  enable  different 
approaches  or  techniques  for  locating  UXO’s.  Finally,  the  motion  exhibited  must  be 
robust  and  stable  due  to  the  nature  of  UXO  environments.  While  traversing  these 
environments,  the  vehicle  should  not  lose  its  precise,  smooth,  and  flexible  motion 
characteristics.  For  these  reasons,  a  rigid  body  vehicle  with  at  least  2  steerable  wheels 
capable  of  semiautonomous  or  autonomous  motions  and  equipped  with  sensors  for 
detecting  UXO’s  was  considered  at  the  abstract  level. 

So,  the  rotary  vehicle  platform  was  chosen,  with  the  addition  of  four  steerable  and 
drivable  wheels  and  a  powerful  computer  system  for  control.  The  four  wheels  have  thick 
tires  and  each  contains  two  motors,  one  for  driving  and  one  for  steering.  Because  of  this, 
three  degrees  of  freedom  motion  is  possible  which  allows  for  motion  flexibility.  The 
independently  driven  four  wheels  aids  in  providing  stronger  traction  than  any  other 
wheeled  vehicle  allowing  for  the  negotiating  of  uneven  slopes  ,  soft  soil,  or  rough  terrain. 
The  vehicle  itself  also  provides  the  capability  for  further  expansion  of  the  system  which 
will  give  it  the  full  capability  of  fulfilling  all  aspects  of  the  UXO  mission.  Chapters  III 
and  IV  expand  on  the  details  of  the  system  architecture  regarding  the  hardware  and 
software  of  this  rotary  vehicle. 


5 


B.  MOTION  MODES 


Due  to  the  uniqueness  of  Shepherd’s  independent  4- wheel  motion  of  360  degrees, 
several  modes  of  motion  are  possible.  The  possible  vehicle  motions  are: 

•  Tangential  —  the  vehicle’s  change  in  direction  of  movement  is  equal  to  its 
change  in  heading. 

•  Constant  orientation  ~  the  vehicle’s  heading  is  constant  regardless  of  the 
change  in  the  direction  of  movement. 

•  Complex  which  falls  into  neither  of  the  above. 

•  Rotation 

At  this  stage  of  development  of  Shepherd  the  constant  orientation,  complex,  and  rotation 
motion  modes  have  been  implemented  with  work  proceeding  on  the  tangential  motion 
mode. 

The  user  interface  menu  has  a  list  of  specific  motions  designed  for  the  vehicle, 
which  encompasses  the  typical  vehicle  motions.  This  list  includes  1-Stop,  2-Straight 
motion  (autonomous),  3-Straight  motion  by  joystick,  4-XY-motion  by  joystick,  5-Rotate, 
6-Sinusoidal,  7-Tomado  (external),  8-Tomado  (internal),  9-Tangential,  0-Exit,  a- 
Tangential  motion  n,  and  t-Test  motion.  The  sinusoidal  motion  is  an  implementation  of 
the  constant  orientation  while  the  tornado  motions  are  an  implementation  of  the  complex 
vehicle  motion.  Both  tangential  motions  are  an  attempt  to  implement  the  tangential 
vehicle  motion.  Further  coverage  will  be  given  to  several  of  these  motions  in  more 
detail. 
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III.  SHEPHERD  SYSTEM  HARDWARE 


A.  OVERVIEW 

The  Shepherd  system  hardware  consists  of  the  mobile  platform  and  the  shepherd 
on-board  computer  system.  The  mobile  platform  is  the  “mechanical”  part  of  Shepherd, 
which  provides  motion  and  is  directed  by  the  on-board  computer  system.  The  Shepherd 
on-board  computer  system  provides  the  computing  power  required  controlling  and 
directing  Shepherd.  Figure  3.1  provides  a  global  perspective  of  the  Shepherd  System 
Hardware.  Shepherd  has  four  wheels,  which  are  controlled  independently.  Each  wheel 
has  two  motors;  one  for  driving  the  wheel  and  the  other  for  steering.  Moreover,  the 
steering  capability  for  each  wheel  exceeds  360  degrees.  The  maximum  driving  speed 
(determined  empirically)  is  approximately  87  centimeters/second.  The  unique  mix  of 
driving  and  steering  capability  is  what  provides  the  challenge  and  motion  flexibility  of 
this  vehicle. 

Shepherd  has  a  mass  of  150  kilograms  and  is  built  to  form  a  square  chassis 
(frame).  Shepherd’s  wheels  are  centered  on  the  comers  of  the  square  leading  to  very 
elegant  calculations,  as  you  will  see  later.  Shepherd’s  Alternating  Current  (AC)  electric 
motors  are  powered  by  12  batteries,  which  are  charged  by  an  external  AC  source  through 
converter.  Figure  3.1  is  also  a  transparent  view  of  Shepherd  from  above  which  shows  the 
Central  Processor  Unit  (CPU),  input/output  (I/O)  boards,  servoamplifiers,  batteries,  and 
wheel  unit  assembly. 


B.  THE  MOBILE  PLATFORM 

The  mobile  platform  consists  of  the  vehicle  body.  The  vehicle  body  includes  the 
vehicle’s  frame,  motors,  encoders,  servocontrollers,  gears,  wheels,  tires,  and  power 
supply.  The  on-board  computer  system  is  not  considered  part  of  the  mobile  platform. 
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Wheel  assembly 


Figure  3.1:  A  transparent  view  of  Shepherd  from  above.  Showing  the  “relative”  location  of  the 
CPU,  and  I/O  boards,  servoamplifiers,  batteries,  and  wheel  assembly.  The  relative  position  of  the 
vehicle  components  is  subject  to  change  as  the  vehicle  is  modified. 


1.  Motors  and  Encoders 

The  eight  motors  and  their  corresponding  shaft  encoders  used  in  Shepherd  are 
from  Yamayo  Electric,  Inc.  These  motors  allow  Shepherd  to  reach  a  “theorectical” 
maximum  driving  speed  of  4  kilometers  per  hour  (km/h),  and  a  rate  of  1  revolution  per 
second  about  the  steering  axis  (Ref.  7).  Figure  3.2  provides  the  characteristics  for  the 
driving  and  steering  motors. 
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Servomotor  Characteristics 

Driving  Motor 

Steering  Motor 

Nominal  Torque 

1.274  N-m 

0.32  N-m 

Maximum 

Torque 

3.84  N-m 

0.98  N-m 

Nominal 

Rotation  Rate 

3000  rpm 

3000  rpm 

Maximum 

Rotation  Rate 

4500  rpm 

4500  rpm 

Size 

60  X  123.5  mm 

54  X  86  mm 

Weight 

1.7  Kg 

0.74  Kg 

Power  (AC) 

400  W 

100  W 

Figure  3.2:  Servomotor  characteristics  for  Shepherd. 

Figure  3.3  illustrates  the  relative  motor  (M1-M7)  position  on  the  vehicle  and  the 
general  orientation  of  the  vehicle  (e.g.,  front). 


Shepherd  Rotary  Vehicle 
Motor  diagram 


Figure  33:  Shepherd  motor  diagram 
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2. 


Servomotor  Controllers 


The  servomotor  controllers  actually  provide  the  commanded  voltage  and  current 
to  the  driving  and  steering  motors  to  effect  motion.  The  importance  of  the  servomotor 
controllers  can  not  be  understated;  the  values  written  to  the  digital  output  board  (and  read 
from  the  digital  input  board)  are  within  the  acceptable  range  of  the  controllers.  Also,  the 
interface  specification  matches  the  range  for  the  motors  used  on  Shepherd  and  input 
signal  voltage  corresponds  to  the  range  allowed  for  the  driving  and  steering  motors.  Later 
sections  of  this  document  will  show  that  the  voltage  produce  by  the  batteries  is 
approximately  144  volts,  which  is  within  the  acceptable  range  of  the  controllers.  Figure 
3.4  contains  the  characteristic  and  interface  data  for  the  servomotor  controllers. 


Servo  Motor  Controller  Specifications 

Motor  Capacity 

400  W 

Interface  Specification 

3000  rpm/5000  rpm 

Output  Current 

8A 

Control  Method 

PWM 

Input  Control  Voltage 

DC  +  120-150  V 

Input  Signal  Voltage 

DC  +/-  10V 

Input/Output  Signal 

8  bit 

Figure  3.4:  Servomotor  controller’s  characteristic  and  interface  data. 


3.  Gears 

Shepherd’s  reduction  gear  system  contains  flat  gears,  planetary  gears,  and  bevel 
gears.  This  gear  configuration  has  a  1:50  gear  ratio  for  both  driving  and  steering  [Ref.  8]. 
Figure  3.5  provides  a  transparent  view  of  the  flat  gears  in  the  wheel  assembly.  Due  to  the 
gear  configuration,  when  the  wheels  are  used  for  steered  then  some  driving  is  also 
initiated.  The  aforementioned  driving  is  cancelled  by  applying  the  required  amount  of 
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“opposite”  driving.  And  this  “opposite”  driving  is  based  on  the  1:50  gear  ratio  and  is 
handled  in  the  Shepherd  code  (Appendix  J  , Consolidated  header  files,  line  360). 

Figure  3.6  is  a  side  cut  away  of  the  wheel  assembly.  This  cut  away  shows  the 
gears  involved  in  transferring  force  from  the  motors  to  the  wheels.  Gear  ratio  and  force 
calculations  can  be  obtained  from  [Ref.  8],  Also,  mounted  one  of  the  flat  gears  is  a  “hah” 
sensor,  which  Shepherd  uses  to  determine  if  wheel  is  aligned. 


Figure  3.5:  Flat  gears  in  the  wheel  assembly  (transparent  view  from  the  top) 
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Figure  3.6:  Side  cut  away  of  the  motors  and  gears  used  in  the  Shepherd  driving  and 
steering  mechanism  (diagram  not  to  scale). 

4.  Wheels  and  Tires 

Shepherd’s  wheel  diameter  is  400  millimeters  (mm),  and  the  suspension  travel 
range  is  100  mm.  Shepherd  has  somewhat  ruggedized  tires,  with  a  tire  friction  factor  of 
.5.  The  maximim  tire  pressure  has  been  calculated  as  49.8  pounds  per  square  inch  (psi); 
where  36  psi  is  currently  used. 
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5. 


Power  Supply  System 


Shepherd’s  power  supply  consists  of  twelve  (12-volt)  batteries  connected  serially. 
The  voltage  generated  from  the  batteries  is  between  144-150  volts  (within  the  servomotor 
specifications).  The  batteries  have  been  used  for  periods  up  to  two  (2)  hours  without  any 
noticeable  degradation  of  performance.  Figure  3.7  shows  the  switches  required  for  the 
operational  settings  of  charge,  run  (battery),  run  (external)  and  run  (extemal/charge).  The 
following  are  valid  switch  settings  for  Shepherd. 

•  Charge:  1-SW-COFF 

2  -SW-B  ON 

3  -SW-A  ON 

•  Run  (battery):  1  -SW-A  OFF 

2  -SW-C  ON 

3  -SW-B  ON 

•  Run  (external):  1  -SW-B  OFF 

2 -SW-C  ON 
3  -SW-A  ON 

•  Run  (extemal/charge):  1  -SW-A  ON 


2  -SW-B  ON 

3  -SW-C  ON 


Figure  3.7:  Shepherd  power  supply  switch  diagram 
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Figure  3.8:  Simplified  schematic  diagram  of  Shepherd  power  supply.  Showing  both  an  external 
AC  source  and  the  12-volt  batteries  serially  connected. 

An  AC  source  (1 15-Volts)  can  be  used  to  run  Shepherd  or  to  charge  the  batteries 
(accomplished  by  the  AC/DC  converter).  Figure  3.8  is  a  global  schematic  of  the 
Shepherd  power  supply  [Ref.  9]. 


C.  SHEPHERD  ON-BOARD  COMPUTER  SYSTEM 

The  Shepherd  vehicle’s  system  design  is  illustrated  by  Figures  3.9,  and  is  broken 
down  into  the  hardware  and  software  components  both  of  which  will  be  explained  in 
greater  detail  in  later.  The  hardware  system  is  a  combination  of  the  mobile  platform,  an 
on-board  computer  system,  servo  drivers,  batteries,  and  a  laptop  computer  for  a  real-time 
I/O  device.  The  computer  system  consists  of  a  Taurus  board  housing  two  Motorola 
CPUs  which  are  68040  and  68030,  a  digital  to  analog  board,  a  digital  input  board,  a 
digital  output  board,  a  digital  counter  board;  and  a  Versa  Module  European  bus 
(VMEbus)  based  on  Motorola  architecture.  Servo-controllers  are  connected  to  these  I/O 
boards.  Motor  encoders  are  connedcted  to  the  counter  board. 
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User 


Figure  3.9:  Diagram  of  the  Shepherd  on-board  computer  system. 
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1. 


Taurus  Board 


The  Taurus  is  a  dual-processor,  and  dual  bus  architecture,  VME  single  slot,  single 
board  computer  [Ref.  11].  The  primary  computing  engine  is  a  Motorola  68040  processor 
running  at  25  MHz.  The  second  processor  on  Taurus  is  the  Motorola  68030.  Although, 
taurus  supports  several  real-time  operating  systems,  an  in-house  operating  kernel,  SRK, 
was  developed  (chapter  IV).  Taurus  also  takes  advantage  of  the  direct  memory  access 
(DMA)  functions  provided  by  Ethernet,  SCSI,  and  Intelligent  Serial  Controllers  to  DMA 
into  main  direct  access  memory  (DRAM)  through  an  isolation  gateway  between  the 
M68040  bus  and  the  M68030  bus.  Moreover,  Taurus  acts  as  a  fully  functional  VMEbus 
controller  and  may  operate  in  Slot  1  of  the  VMEbus  back  plane  (this  is  the  case  for 
Shepherd).  Hence,  the  Taurus  board  is  a  powerful  VMEbus  engine  and  supports  the 
requirements  for  a  real-time  operating  system  and  a  completely  self-contained  computing 
environment.  Taurus  features: 

•  25  MHz  M68040  Processor 

•  Burst  Transfers 

•  Ethernet  and  SCSI  with  on  chip  DMA 

•  1 6  Megabytes  of  DRAM  main  Memory 

•  4  Megabytes  of  EPROM 

•  1  Megabyte  of  Flash  EPROM 

•  25  MHz  M68030  Processor 

•  6  Serial  Ports:  4  (RS-232-D  Intelligent  Ports  with  DMA),  and  2  using  a 
68c681  device 

•  32  Parallel  I/O 

•  1 1,  16  bit  Timers  (cascadeable  into  combinations  of  up  to  80  bits) 

•  Interprocessor  Mailbox 

•  Dynamic  Bus  Sizing 

•  Real-time  clock  with  battery  back  up 

•  Watchdog  Timer  and  8  KB  of  battery  back-up  Static  RAM 
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Why  was  the  Taurus  board  chosen  for  this  project?  In  addition  to  the 
aforementioned  characteristics,  the  board  uses  the  M68040  processor.  In  previous 
development  of  the  Yamabico-11  [Ref.  12]  robot,  the  M68020  was  used.  Motorola 
claims  that  its  M68000  series  chips  are  backward  compatible.  This  research  has 
concluded  that  this  is  mostly  true,  however  on  some  key  issues  (math  functions  and 
assembly  code)  this  has  not  proven  to  be  the  case.  These  issues  will  be  revisited  in 
chapter  IV.  Again  a  key  asset  of  the  Taurus  board  is  how  elegantly  and  logically  internal 
and  external  interrupts  are  handled  (Figure  3.10). 


VMEbus  Interrupts 

o 


Interrupts  from 
On-Board  Devices 
-SCSI 
-Ethernet 


i=> 


ISM 

Interrupt  Steering  Mechanism 


r . ""*•» 

IRQ-x 


VIC068/VIC64 


M68030 


IRQ-6 


M 

6 

8 

0 

4 

0 


Figure  3.10:  Interrupt  handling  diagram  for  the  Taurus  board  [Ref.  11] 


The  Taurus  board's  communications  facility  adds  flexibility  to  the  implementation 
of  RS-232  or  Ethernet.  The  M68040  chip  is  a  very  versatile  processor  and  powerful 
processor,  and  can  perform  14  different  operations  at  a  given  time.  That  is  6  operations 
by  the  Integer  Unit  (IU),  3  operations  by  the  Floating-Point  Unit  (FPU),  4  by  the  Memory 
Management  Unit  (MMU),  and  a  Bus  interface  operation.  The  number  of  timers  on  the 
Taurus  board  is  also  of  a  great  benefit,  moreover  Timer  5  will  be  discussed  in  chapter  IV. 
The  M68040  is  highly  parallel,  with  a  6-stage  integer  pipeline,  that  when  filled,  will 
execute  an  instruction  for  every  clock  cycle.  Moreover,  each  of  the  MMUs  can 
accomplish  a  cache  access  and  address  translation  concurrently. 
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2. 


Digital  To  Analog  Board 


The  Acromag  Series  9210  Analog  Output  Board  (AVME9210)  provides  the 
means  for  connecting  and  driving  analog  circuits  with  outputs  from  the  VMEbus  for  the 
Shepherd  system  [Ref.  13].  The  board  has  8  channels;  each  channel  has  a  12-bit 
resolution.  A  DAC  per  channel  is  used  for  signal  accuracy.  The  DACs  are  set  up  to 
accept  either  straight  binary  or  two’s  compliment  data.  The  board  has  five  programmable 
ranges  for  output  voltages;  however,  +/-  10-Volts  will  be  used  with  shepherd  because  it  is 
a  direct  mapping  to  the  maximums  for  the  servomotors  inputs.  Characteristics  of  the 
AVME9210  are  as  follows: 

•  1 2  bit  output  resolution 

•  individual  DAC  per  channel 

•  8  channels  per  board 

•  Byte  or  Word  data  transfers 

•  Power  up  reset 

•  Pass/Fail  status  indicators  on  the  front  panel 


DAC  Data  Register 

Data  Register 

Base  Address  Offset 

Channel  0 

+82H 

Channel  1 

+84H 

Channel  2 

+86H 

Channel  3 

+88H 

Channel  4 

+8  AH 

Channel  5 

+8CH 

Channel  6 

+8EH 

Channel  7 

+90H 

Figure  3.11:  DAC  data  register,  8  channels  of  output.  Each  channel  is  a  digital  to  analog  converter. 
A  two  byte  address  is  reserved  for  each  data  register. 

A  single  channel  represents  a  driving  or  steering  motor  in  Shepherd  [Appendix  E, 
Motor.c,  line  228].  The  status  control  register  controls  the  pass/fail  light.  The  Shepherd 
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code  toggles  the  pass  fail  light  in  some  instances  to  ensure  the  system  and  code  are 
functioning  properly.  There  is  a  memory  location  for  the  board  status  indicator  flags  and 
reset.  This  memory  location  is  one  byte  in  length  and  is  located  at  base  address  +81H. 
During  the  early  manual  testing  this  status  control  register  was  used  exclusively  to 
accomplish  resetting  the  AVME9210.  Also,  each  of  the  aforementioned  two  byte  DAC 
data  registers  (Figure  3.12)  are  set  up  as  follows: 


MSB 


LSB 


Dll 


DIO 


D9 


D8 


D7 


D6 


D5 


D4 


D3 


D2 


D1 


DO 


X 


X  X 


12  bits 


4  bits 


Bit  DAC  Data 


Undefined 


Figure  3.12:  DAC  data  register  (16  bit).  Most  significant  bit  (MSB)  and  least  significant  bit  (LSB). 
The  12  bits  are  a  direct  mapping  of  input  values  to  the  board,  integer  range  [1023,  -1024]. 


The  AVME9210  is  located  at  base  address  0xffff0400  and  is  represented  in  the 
Shepherd  code  by  the  “label”  VME9210. 


Motor  and  DA  Board  Address  Ma] 

PPing 

Driving  Motor  and 

Address 

Steering  Motor  and 

Address 

Wheel  1 

Ml 

VME9210  +  0x0082 

M5 

VME9210  +  0x008A 

Wheel  2 

M2 

VME9210  +  0x0084 

M6 

VME9210  +  0x008C 

Wheel  3 

M3 

VME9210  +  0x0086 

M7 

VME9210  +  0x008E 

Wheel  4 

M4 

VME9210  +  0x0088 

M8 

VME9210  +  0x0090 

Figure  3.13:  AVME9210  address  to  “physical”  motor  mapping. 
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3. 


Digital  Input  Board 


The  Acromag  Series  9421  Isolated  Digital  Input  Board  (DIB)  provides  the  means 
for  connecting  the  Digital  DC  inputs  the  VMEbus  for  the  Shepherd  system  [Ref.  14]. 
The  DIB  board  isolates  all  digital  inputs  from  the  VMEbus  for  up  to  250V  AC,  or  350V 
DC  on  a  continuous  basis  (falls  within  the  constraints  of  the  Shepherd  servomotors).  The 
pass/fail  light  on  this  board  is  similar  to  the  one  used  in  the  digital  to  analog  board 
previously  mentioned.  And  the  DIB  also  has  the  input  channel  on  light  as  well.  The 
board  has  64,  1  bit  channels  configure  as  four,  16  bit  words.  The  inputs  can  be  bi-polar 
(with  polarity  being  +/-  or  -/+  at  either  end  of  the  channel).  The  bi-directional  polarity 
allows  Shepherd  to  use  this  for  changing  the  direction  of  wheel  driving  or  the  direction  of 
steering  with  a  change  of  input  polarity.  The  DIB  has  the  base  address  of  OxffffOOOO  and 
the  “label”  VME9421  is  used  in  the  code  [Appendix  J,  Consolidated  header  files,  line 
386], 


4.  Digital  Output  Board 

The  Microsystems  International  Corporation  32-bit  Optically  Coupled  Digital 
Output  Board  (VMIVME-2170A)  consists  of  VMEbus  compatibility  logic,  data  output 
control  logic,  four  8-bit  output  registers,  and  32  bits  of  isolated  outputs.  The  VMEbus 
logic  contains  address  decoding  logic  and  data  transfer  control  logic,  which  provides  for 
8-  or  16-  bit  data  transfers  in  the  “short”  I/O  address  space.  The  data  output  control  logic 
selects  byte  or  word  transfers  to  the  32  optically  isolated  channels.  The  Shepherd 
research  group  spent  many  hours  attempting  to  master  this  logic — a  key  problem  was 
how  to  determine  where  the  least  significant  value  was  for  each  data  register.  However, 
Thorsten  Leonardy’s  efforts  paved  the  way  for  a  consistent  and  logical  method  of  writing 
to  the  aforementioned  registers.  From  this  the  Shepherd  group  was  able  to  selectively 
choose  combinations  of  motors  for  steering  and  driving  [Appendix  J,  Consolidated 
header  files,  lines  181-192].  Figure  3.14  shows  the  register  bit  definitions  [  Ref.  15]. 
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$XXX0  Data  Register  < 

0 

Bit  15 

Bit  12 

Bit  11 

Bit  10 

Bit  9 

Bit  8 

Output  Data 

OD31 

OD30 

OD29 

OD28 

OD27 

OD26 

OD25 

OD24 

$XXX1  Data  Register  1 

Bit  7 

Bit  6 

Bit  5 

Bit  4 

Bit  3 

Bit  2 

Bit  1 

Bit  0 

Output  Data 

OD23 

OD22 

OD21 

OD20 

OD19 

OD18 

OD17 

OD16 

$XXX2  Data  Register  2 

Bit  15 

Bit  14 

Bit  13 

Bit  12 

Bit  11 

Bit  10 

Bit  9 

Bit  8 

Output  Data 

OD15 

OD14 

OD13 

OD  12 

OD1 1 

ODIO 

OD9 

OD8 

$XXX3  Data  Register  3 

Bit  7 

Bit  6 

Bit  5 

Bit  4 

Bit  3 

Bit  2 

Bit  1 

BitO 

Output  Data 

OD7 

OD6 

OD5 

OD4 

OD3 

OD2 

OD1 

ODO 

Figure  3.14:  Register  Bit  Definitions 


The  output  board  has  the  base  address  of  OxffffffOO,  which  is  represented  in  the 
code  by  the  “label”  VME2170. 


VME2170  Bit  Assignment 

Register  3 

Register  2 

Register  1 

Register  0 

OOOD 

0000 

1001  0C10 

0100  1001 

0010 

0100 

▲ 

"Unused 


I _ IL 

/ 


.  ;  t  , 

Whe^l  4  Wheel  3  Wheel  2  Wheel  1 
Steering  Steering  Steering  Steering 


V' 

lc 


JU 


Wheel  4  Wheel  3  Wheel  2  Wheel  1 
Driving  Driving  Driving  Driving 


Figure  3.15:  VME2170  bit  assignment  Note  3  bit  assignment  for  driving  or  steering  motor  selection. 
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Figure  3.15  indicates  that  “masks”  could  be  written  to  the  VME2170  base  address 
and  used  for  motor  selection.  Hence,  writing  a  mask  for  0x00000004  would  select,  motor 
1  (Ml)  in  wheel  1  for  driving.  And  writing  a  mask  for  0x00004000  would  select,  motor 
5  (M5)  in  wheel  1  for  steering.  Moreover,  using  a  mask  for  0x00924924  all  motors  can 
be  selected  (using  a  “logical  And”  on  the  true  values  for  each  motor  selected). 


5.  Counter  Board 

Shepherd  uses  the  Green  Spring  IP-Quadrature  Four  Channel  Quadrature  Decoder 
or  counter  board  [Ref.  16].  The  counter  board  reads  the  signal  produced  by  the  encoders 
(see  section  m.B.1  of  this  document);  this  signal  is  “index”  pulse  once  per  revolution  to 
provide  absolute  position  information.  There  are  four  channels  on  the  counter  board; 
each  channel  has  three  inputs.  The  inputs  are  normally  called  X,  Y,  Z,  and  the  board 
inputs  are  X  and  Y.  Z  is  the  control  or  index  input.  Each  channel  has  a  24-bit  up/down 
counter  block,  24-bit  capture/match  register  and  a  24-bit  output  latch  allow  for  an 
accurate  “on  the  fly”  reading  of  Quadrature  position  values.  The  up/down  count  direction 
in  the  counter  board  is  controlled  by  the  relative  phase  of  the  X  and  Y  inputs.  Count 
direction  can  be  reversed  by:  reversing  the  mechanical  motion  direction,  reverse  the 
connections  for  X  and  Y,  reverse  the  X  polarity  bit,  or  reverse  the  Y  polarity  bit. 
Shepherd  uses  a  function  called  “readEncoders”  which  is  an  excellent  example  of 
completing  three  consecutive  8-bit  reads  from  the  counter  board  and  catenation  the  8-bit 
segments  into  a  single  24-bit  position  reading  [Appendix  E:,  Motor.c,  lines  300-356  and 
606-620],  The  counter  board’s  base  address  address  is  0xffff6000,  and  is  represented  by 
the  label  VMECTR1  in  the  Shepherd  code. 
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IV.  SHEPHERD  SOFTWARE  SYSTEM 


A.  OVERVIEW 

The  Shepherd  software  system  consists  of  the  software  development  environment 
(including  GCC  version  2.7.2. 1  compiler),  Shepherd  Real-time  Kernel  (SRK),  and  the 
firmware  on  the  Taurus  board.  Shepherd  software  is  developed  in  the  “C”  language  on  a 
Unix  workstation,  and  the  code  is  cross-compiled  using  switches  with  the  GCC  compiler 
to  ensure  viability  on  the  Taurus  board  (step  by  step  instructions  are  in  the  Shepherd 
Operating  Manual,  Appendix  K).  The  code  is  in  “S”  record  format  and  is  transferred  (via 
Ethernet)  to  a  laptop  computer,  which  is  used  as  the  user  interface.  When  the  user  is 
ready  to  test  or  run  a  program,  it  is  then  download  via  RS232  to  the  RAM  on  the  Taurus 
board  using  the  Taurus  bug  or  firm-ware  on  the  Taurus  board.  The  SRK  has  real-time 
timer  control.  The  timer  interrupts  are  set  for  10  milliseconds,  but  can  be  modified  to 
suite  user  needs  and  requirements.  SRK’s  central  motion  control  sections  are  shepherd.c, 
user.c,  and  the  driver  routine  (all  of  which  will  be  discussed  in  great  detail  later).  The 
user  interface  viewed  from  the  laptop  computer  is  also  generated  from  the  code  in  user.c 
and  other  I/O  code  segments.  The  overall  software  environment  and  firmware  works 
together  to  form  a  tightly  coupled  and  low  overhead  “operating  kernel”  that  is  the  SRK. 
The  use  of  the  SRK  allows  the  user  to  control  (i.e.,  timer  interrupt,  motion  control,  and 
user  interface)  the  Shepherd  rotary  vehicle. 


B.  SOFTWARE  ENVIRONMENT 

The  software  environment  (minus  the  SRK)  includes  the  following:  Bug  monitior, 
S-records,  and  Software  development  environment. 

1.  Bug  Monitor 

The  “taurus  bug”  is  a  powerful  debugging  and  evaluation  tool,  and  is  firmware  on 
the  Taurus  board  [Ref.  20],  It  has  facilities  for  loading  and  executing  user  programs 
under  complete  operator  control  and  evaluation  (and  is  used  extensively  in  Shepherd 
development).  The  “taurus  bug”  includes  commands  that  allow  the  user  to  display 
memory,  modify  memory,  set  and  remove  break  points,  an  assembler/disassembler,  and  a 
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system  self  test  capability  that  verifies  system  integrity  upon  power  up.  The  “taurus  bug” 
also  has  various  routines  to  handle  some  I/O,  string  functions,  and  data  conversion  via  the 
TRAP  #15  handler  on  the  Taurus  board  (used  in  Appendix  C,  lines  535-548).  Moreover, 
on  power  up,  all  static  variables  are  set  to  default  states,  the  break  point  table  is  cleared, 
all  target  registers  invalidated,  I/O  character  queues  cleaned,  the  vector  interrupt  table  is 
written  to  RAM,  and  all  on-board  devices  (serial  ports,  timers,  etc.)  are  cleared  or  reset. 
Taurus  bug  also  has  a  system  reset  and  abort  feature.  The  system  reset  completely  re¬ 
initializes  the  board  and  the  abort  feature  captures  a  snapshot  of  the  processors  present 
state — allowing  the  use  of  stack  pointers,  and  the  program  counter  to  help  determine 
errors  (the  hardest  way  to  debug).  The  “taurus  bug”  was  a  very  valuable  tool;  however, 
at  times  it  was  difficult  to  use.  And  tracing  through  assembly  code  to  resolve  a  problem 
using  the  “Trace”  function  and  the  symbol  table  can  take  numerous  man-hours  -without 
immediately  yielding  a  positive  result.  A  very  important  function  of  “taurus  bug”  is  the 
loading  capability.  The  use  of  the  “Lo”  command  to  place  executable  code  in  memory  is 
key  in  the  development  process.  The  “S”  records  are  downloaded  to  the  Taurus  board  in 
this  manner. 


2.  “S”  Records 

The  S-record  format  was  devised  by  Motorola  for  output  modules.  Its  key 
purpose  was  encoding  programs  or  data  files  in  a  printable  format  for  the  transportation 
between  computer  systems.  Hence,  providing  a  way  of  visiually  monitoring  the 
transportation  processs  and  a  method  of  quicky  editing  the  code  if  required.  S-records 
are  character  strings  made  of  several  fields  which  identify  the  record  type,  record  length, 
memoery  address,  code  data  and  checksum  (see  Figure  4.1). 


Field 

Printable  Characters 

Contents 

Type 

2 

S0-S9 

Record  Length 

2 

Character  pairs  in  record 

minus  type 

Address 

4,  6,  or  8 

The  2, 3,  or  4  byte  address 

at  which  the  data  is  loaded 

into  memory 
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Field 

Printable  Characters 

Contents 

Code/Data 

0-2n 

From  zero  to  2n  bytes  of 

executable  code, 

descriptive  information, 

or  loadable  data. 

Checksum 

2 

Least  significant  byte  of 

l’s  complement  of  the  sum 

of  the  values  represented 

by  the  pairs  of  characters 

making  up  records  length, 

address  and  the  code/data 

fields.  Used  for  error 

checking 

Figure  4.1:  S-record  content  chart  [Ref.  20]. 


S-records  module  may  contain  the  following  types  (and  many  more):  SO  (header 
data),  record  containing  address  where  code  is  to  reside  in  memory  (SI,  SI,  or  S3),  S5 
(the  number  of  records  transmitted  per  block),  and  the  termination  record  (S7,  S8,  or  S9). 
A  typical  S-record  might  look  like  this: 

S0060000484452 1 B 

S113000284F245F2212226A000424290008237C21 
S 1 1 300 1 0000200080008262900 1853812341001813 
S9030000FC 

A  detailed  byte  wise  explanation  of  S-records  is  contained  in  Ref.  20.  The  S-records  for 
shepherd’s  development  are  generated  during  the  linking  and  loading  process.  A  special 
switch  is  used  that  allows  the  creation  of  a  File  named  “shepherd.TXT”.  The 
“shepherd.TXT”  file  contains  the  S-records  to  be  downloaded. 
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3.  Software  Development  System 


As  mentioned  in  this  section's  overview  the  compiler  used  is  the  GCC  2.7.2. 1. 
This  compiler  posed  many  problems  for  SRK  development.  One  of  the  biggest  obstacles 
was  the  passing  of  “composite”  structures.  Structure  values  had  to  be  placed  into  dummy 
variables  (Appendix  D,  lines  138-140)  in  order  to  get  the  code  to  execute.  This  was 
especially  odd  because  the  code  compiles,  but  will  not  execute  (if  the  dummy  variables 
are  not  used).  Also,  several  of  the  compiler  switches  that  supposedly  allow  mathematical 
code  (that  was  previously  valid  for  the  M68020  with  math  co-processor)  would  simply 
not  compile.  An  inordinate  amount  of  time  was  spent  trying  to  resolve  this  issue  because 
one  of  the  initial  precepts  of  the  project  was  that  old  code  from  the  Yamabico-11  robot 
system  would  be  portable.  There  are  still  anomalies  that  work  arounds  were  developed 
for.  For  instance,  there  is  a  “square  root”  function  that  compiles  and  works  in  every 
environment  (other  compilers)  yet  would  never  function  when  compiled  using  GCC 
2.7.2. 1.  Moreover,  because  the  compiler  does  not  have  libraries  for  I/O  or  standard  math 
functions,  these  had  to  be  derived  to  support  the  M68040  and  Taurus  board  (again  taking 
an  inordinate  amount  time).  The  compiler  did  serve  its  purpose — because  its  freeware 
(the  budget  did  not  allow  for  commercial  compilers). 

One  switch  that  did  work  was  -m68040,  which  allowed  the  generation  of  M68040 
specific  code.  The  "makefile”  makes  use  of  the  -m68040  (segment  below): 

shepherd.o  :  shepherd.c 

gcc  -c  -m68040  -o  shepherd.o  shepherd.c 

timer.o :  timer.c 

gcc  -c  -m68040  -o  timer.o  timer.c 

user.o  :  user.c 

gcc  -c  -m68040  -o  user.o  user.c 

motor.o :  motor.c 

gcc  -c  -m68040  -o  motor.o  motor.c 
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The  early  testing  also  required  a  complete  understanding  of  the  “a.out”  generated 
from  the  assembler  and  the  link  editor  (the  link  editor  makes  “a.out”  executable  files). 
The  “a.out”  (Figure  4.2)  consists  of:  a  header,  program  text,  program  data,  text  and  data 
relocation  information,  a  symbol  table,  and  a  string  table  (the  last  three  sections  may  be 
omitted  if  the  program  is  loaded  with  the  -s  option.  All  of  the  aforementioned 
information  was  useful  because  our  earliest  a.outs  were  in  the  wrong  format,  hence  not 
useable  by  the  M68040  due  to  using  non-functioning  compiler  switches. 


A.OUT 


Mage  Number  j  Size  text  j  Size  data  I  Other 


TEXT 


DATA 


BSS  STACK 
SEGMENT 


\  Header  data 


Figure  4.2:  A  generic  A.OUT  format  Note  many  features  of  the  A.OUT 
are  left  off,  such  as  symbol  table,  entry  point,  dynamic,  and  machine  type. 

The  final  element  to  be  discussed  is  how  the  link  editor  is  used.  First,  we  must 
discuss  the  DRAM  Memory  map  (Figure  4.3).  The  Taurus  board  documentation  [Ref. 
21]  warns  that  accessing  the  memory  below  $10000  (Hex),  hence  the  memory  the  format 
used  in  Figure  4.3  with  16  Megabytes  as  the  upper  bound. 
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DRAM  Memory  Map 


$100000 

or 

16  MB 


END  DRAM 


Region  Available  for 
User  program 


$10000 

$0 


Region  used  by 
“taurus  bug” 


Start  user  DRAM 


Start  DRAM 


Figure  4.3:  DRAM  memory  map.  It  should  be  noted  that  to  be  able  to  write  to 
the  DRAM  the  Parity  ISM  register  must  be  disabled  or  the  block-  fill  command 
(taurus  bug)  used  to  clear  the  region  required  [Ref .  12,  and  22].  A  detailed 
example  is  in  the  Shepherd  Operating  Manual  (Appendix  K). 

Again  the  Shepherd  makefile  is  illustrative  of  the  linking  process: 

comp:  startup.o  shepherd.o  timer.o  serial.o  math.o  utils.o  utdls030.o  user.o\ 
motor.o  movement.o 

Id  -Ttext  0x10000  -Tdata  0x20000  -Tbss  0x30000  -Map  shepherd.map 
oformat  srec\ 

-o  shepherd.TXT  startup.o  shepherd.o  timer.o  serial.o  math.o  utils.o\ 
utils030.o  user.o  motor.o  movement.o 

The  make  file  shows  the  text  segment  of  the  code  being  loaded  at  0x10000,  the  data 
segment  of  the  code  loaded  at  0x20000,  and  the  upper  bound  for  the  data  at  0x30000. 
Hence,  the  code  is  loaded  within  the  parameters  required  by  the  memory  map.  Earlier  it 
was  mentioned  that  the  S-records  were  generate  by  the  linker — the  “oformat  srec  -o 
shepherd.TXT”  generates  the  required  S-records  for  download  to  the  laptop  PC.  If  the 
“oformat  srec”  switch  is  not  used  then  the  standard  a.out  will  be  generated.  The  -Map 
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switch  also  allows  the  user  to  generate  a  symbol  table  (called  shepherd.map  here)  for  use 
in  debugging.  At  this  point  all  the  underlying  structure  for  SRK  development  is  in  place. 


C.  SHEPHERD  REAL-TIME  KERNEL  (SRK)  ARCHITECTURE 

The  SRK  includes  the  Timer  control,  Motion  control,  and  the  User  interface. 
Figure  4.4  below  illustrates  the  exact  architecture  of  the  motion  control  part  of  the 
system. 


motxmMode 


Figure  4.4:  Shepherd  Motion  Control  Architecture 


1.  Timer  Control 

The  timer  control  is  a  very  integral  part  of  the  system.  As  was  stated  earlier  SRK 
has  a  real-time  modifiable  timer  control.  This  timer  control  is  made  possible  through  the 
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use  of  the  Taurus  board’s  AM9513A  Counter/Timer.  This  Timer/Counter  provides  five 
16-Bit  general  purpose  counters  and  uses  a  4-Mhz  oscillator  as  a  clock  input  with  the 
outputs  connected  to  the  Taurus  Interrupt  structure  for  processing.  The  Timer  5  group  of 
the  AM9513  device  was  utilized  due  to  it’s  compatibility  with  the  68040  Processor  and 
contains  it’s  own  timer  handler. 

The  main  routine  in  controlling  the  timing  and  setting  the  interrupts  is  located  in 
the  timer.c  file  along  with  the  header  file  in  timer.h  (Appendix  F).  This  timer  is 
initialized  and  started  in  the  main  shepherd  routine.  It  continuously  provides  a  10 
millisecond  interrupt  until  the  program  is  terminated.  However,  this  value  could  be 
modified.  This  was  made  possible  by  manipulating  the  data  port  values  of  the  AM9513-1 
device  and  multiplying  those  values  by  factors  of  10000, 1000,  100, 10,  or  1  to  obtain  a  1, 
0.1,  0.01,  0.001,  or  0.0001  second  interrupt  in  that  order.  The  accuracy  of  this  timing 
was  tested  using  an  oscilloscope  and  also  a  frequency  analyzer.  It  was  found  through 
testing  that  the  SRK  when  in  operation  only  utilizes  approximately  2.7  milliseconds  total 
in  handling  all  the  associated  routines.  For  further  information  on  this  counter/timer  see 
reference  21. 


2.  Motion  Control 

Referring  back  to  Figure  4.4,  motion  control  encompasses  several  major  parts 
which  are  woven  into  a  tightly  controlled  structure  for  driving  and  Steering  Shepherd’s 
wheels.  The  bodyMotion  function  takes  as  input  the  mode  of  motion  desired  from  the 
user.  Using  this  mode  and  the  necessary  instructions  programmed,  it  provides  5  inputs  to 
the  wheelMotion  function  which  are:  motion,  speedDot,  thetaDot,  omegaDot,  and 
vehicle.  Motion  is  a  structure  consisting  of  the  user’s  input  of  speed,  theta  (direction  of 
travel  of  the  vehicle),  and  omega  (rotational  speed  of  the  vehicle).  Vehicle  is  a  structure 
consisting  of  the  x  and  y  coordinate  of  the  vehicle  on  some  x-y  plane  for  tracking 
purposes  and  the  heading  of  the  vehicle.  SpeedDot,  thetaDot,  and  omegaDot  all  are  a 
derivation  of  speed,  theta,  and  omega  over  time. 

The  wheelMotion  function  takes  the  given  inputs  along  with  inputs  from  a 
feedback  loop  for  the  actual  positions  of  the  encoders  for  both  driving  and  steering  of  the 
vehicle,  and  performs  calculations  based  on  the  theory  discussed  in  chapter  VI.  The 
results  of  these  calculations  are  then  sent  to  the  driveMotors  function  which  provide  these 


30 


values  to  the  servo  drivers  for  steering  and  driving  the  vehicle  wheels  resulting  in  the 
motion  of  Shepherd. 


3.  User  Interface 

The  user  interface  is  facilitated  through  the  use  of  a  Texas  Instruments  laptop 
computer  running  the  Windows  95  operating  system.  By  utilizing  the  HyperTerminal 
program  accessory  to  connect  the  laptop  to  the  unix  system  via  an  ethemet  connection, 
program  files  can  easily  be  receive  from  the  unix  system  after  being  compiled  and  the 
executable  files  sent  to  the  taurus  board.  After  successful  downloading  of  the  program  to 
the  Taurus  board,  the  user  can  then  control  the  operation  of  Shepherd  through  the  laptop 
keyboard.  Figure  4.5  illustrates  this  user  interface  after  the  program  is  successfully 
downloaded  and  ran.  An  on-screen  menu  is  displayed  with  a  description  of  Shepherd 
motion  that  can  be  initiated  via  keyboard  strokes. 

This  menu  is  located  in  the  file  user.c  (Appendix  C).  It  is  a  simple  character 
definition  which  is  called  from  the  user.c  file.  The  menu  choice  inputs  from  the  keyboard 
are  converted  to  ascii  characters  which  are  interpreted  in  the  user()  routine  switch 
statement  to  launch  the  associated  menu  item  seen  in  figure  4.5  on  the  next  page. 

Also,  part  of  the  user  interface  is  a  joystick.  This  joystick  interface  is  activated 
when  menu  choices  3  and  4  are  selected.  It  controls  Shepherd’s  wheels  for  both  steering 
and  driving.  This  will  be  discussed  in  Chapter  VI. 
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%•  ses2download  -  HyperT  erminal 


Eie  £<£t  View  £«8  icarafer  Help 


d  c?  mm  *ois  w 


SHEPHERD  Hain  Menu  (Last:  Update:  27  Feb  97) 

1 - s - 0 - 5 - 0 - S - 0 - S - 0 - S - 0 - S - 0 - 5 

Please  choose:  Diagnostics 


(1)  Stop 

(2)  Straight  Hot ion  (Autonomous) 

(3)  Straight  Hot ion  by  Joystick 

(4)  XY-Hotion  by  Joystick 

(5)  Rotate 

(6)  Sinusoidal  Motion 

(7)  Tornado  (External  Center  of  Rotation) 

(8)  Tornado  (Internal  Center  of  Rotation) 

(9)  Align  Wheels 
(0)  Exit  Program 


nr _ ...ujarouvi  fwr^TvT^  ~~~ fcrprtt. i  .  frAPC fiunirT VF 

jgE|  Start)  H?WS_FTP^LE131.12ai....||%*e82download  -  Hype 


Figure  4.5:  Display  of  shepherd  menu  for  user  interface. 


BiQ 
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V.  EXPERIMENTAL  RESULTS  ON  MOTION  CONTROL 


A.  OVERVIEW 

Chapters  HI  and  IV  of  this  thesis  discuss  how  each  servomotor  can  be  accessed 
and  voltages  applied.  However,  the  aforementioned  chapters  do  not  explain  how  the 
incremental  inputs  placed  on  the  DA  translate  to  wheel  speed  or  what  the  maximum  and 
minimum  wheel  speeds  are.  So,  how  were  the  maximum  and  minimum  wheel  speeds 
determined?  Moreover,  what  type  “controls”  are  required  to  ensure  that  each  wheel  has 
the  same  driving  velocity  or  angular  velocity?  We  used  a  modified  version  of  the 
“scientific  method”  to  establish  and  carry  out  the  experiments  for  the  Shepherd  vehicle. 

Scientific  Method  and  Approach 


Figure  5.1:  Modified  version  of  the  scientific  method  and  approach  used  for  Shepherd  development 


33 


As  with  any  experiment,  results  must  be  consistent  and  reproducible.  Considering 
the  vehicle  as  a  “closed  system”  observation  of  all  experimental  results  should  lead  to 
deduction,  connection,  or  correlation.  As  is  common  with  the  scientific  method  progress 
can  be  very  rapid  or  slow.  Sometimes  there  were  unexpected  results  (e.g.,  singularities), 
and  in  some  cases  our  dead-end  path  (failed  experiment  segment)  provided  some  very 
important  insights  which  helped  to  improve  vehicle  performance.  An  Ockham’s  razor 
approach  was  taken  when  making  investigations  and  deductions — meaning  that  any 
unknown  phenomena  or  behavior  should  be  explained  in  terms  of  what  is  already  known 
(and  testing  the  simplest  possibility  first). 


B.  WHEEL  DRIVING 

Once  an  understanding  of  the  mechanics  and  providing  the  coded  structures  to 
move  the  wheels  had  been  achieved  the  concept  of  precise  and  controlled  driving  motion 
becomes  the  focus.  The  basic  initial  idea  was  to  measure  the  counts  from  the  counter 
board,  measure  wheel  revolutions  over  time.  Again  reiterating  a  modified  scientific 
method  was  used —  and  apriori  data  such  as  gear  ratio  and  other  engineering  data  were 
used  to  verify  results. 


1.  Developing  Driving  Constants 

The  digitToRadDrive  constant  [Appendix  J,  Consolidated  header  files,  line  364] 
was  the  first  constant  to  be  determined  for  wheel  driving.  The  biggest  problem  with 
developing  this  constant  was  working  around  the  10-millisecond  timer  interrupt  and  the 
lack  of  a  fully  functional  operating  system.  Since  Shepherd’s  SRK  was  developed  from 
the  ground  up  within  the  last  year  it  has  limited  I/O  capabilities,  and  there  is  no  long-term 
storage  on  the  Taurus  board.  What  this  means  is  that  the  results  of  test  programs  would 
have  to  be  sent  to  a  monitor  (VT220)  or  to  a  printer.  We  decided  to  print  to  both  the 
monitor  and  printer.  However,  this  also  yielded  unexpected  results.  The  print  function 
used  too  much  of  the  10  milliseconds  to  allow  for  proper  functioning  of  the  process 
currently  running.  Hence,  we  moved  the  print  function  from  the  real-time  portion  of  the 
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code  to  alleviate  this  problem.  Moreover,  this  became  a  useful  technique  that  was  used 
throughout  Shepherd  development  and  testing. 

The  first  step  in  determining  the  number  of  counts  is  reading  the  counter  board. 
In  the  algorithm  used  in  the  SRK,  the  initial  values  on  the  counter  board  are  read,  stored, 
and  read  again  after  a  complete  revolution  of  the  tired  is  complete;  the  absolute  difference 
between  the  initial  counter  value  and  the  final  counter  is  the  actual  count  take  for  the 
revolution.  It  should  noted  however  that  consecutive  reads  of  the  counters  is  actually 
accomplished  and  the  values  adjusted  for  the  transition  from  Oxffffff  to  0x000000  and 
vice  versa  (24  bits)  [Appendix  E,  Motor.c,  lines  607-621].  An  example  of  the  code  to 
read  the  encoders  for  driving  can  be  seen  below: 

void  readDriveEncoders  (unsigned  long  int  array)]) 

{ 

unsigned  char  *p= (unsigned  char*)VMECTRl ,  cl,  c2,  c3; 

int  ix; 

long  int  temp; 

for  (ix=0;  ix<4;  ix++)  {  /*  read  all  four  motors  subsequently  */ 

*(p+3)=0x03;  /*  load  output  latch  from  counter  * / 

*(p+3)=0x01;  /*  control  register,  initialize  two-bit 

output  latch  */ 

/*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

/*  first  access  to  Output  Latch  Register  reads  least  significant  * / 

/*  byte  first  */ 

cl  =  *(p+l)  &  OxOOff; 
c2  =  *(p+l)  &  OxOOff; 
c3  =  *(p+l)&  OxOOff; 

array[ix]  =  ((unsigned  int)cl)  |  ((unsigned  int)c2  <<  8)  | 

((unsigned  int)c3  <<  16); 

p=p+4;  /*  increment  pointer  for  next  counter  */ 

> 

return; 


35 


}  /*  end  of  readDriveEncoders  */ 


Secondly,  a  method  to  display  the  enconder  data  is  required.  The  SioOut 
[Appendix  E:,  Motor.c,  lines  483-490  and  381-399]  display  routine  from  SRK  was  used 
for  this,  but  failed  because  the  time  required  to  display  the  data  exceeded  10  milliseconds. 
So,  we  moved  the  display  “call”  for  the  function  from  the  real-time  portion  of  the  code. 
In  SRK,  immediately  following  the  “driver”  routine’s  call  the  following  type  routine 
would  be  placed  in  the  main  “user”  routine: 

while(l) 

{ 

while (edCounter%200  !=  0) {}; 
displayCountQ; 

}; 


The  aforementioned  routine  would  print  based  on  the  edCounter  and  the  timer.  The 
counter  being  incremented  every  10  milliseconds.  Hence  the  value  is  displayed  every  2 
seconds  in  real-time.  The  key  is  the  fact  that  the  printing  is  being  executed  outside  of  the 
driver  routine. 

Now  all  the  elements  are  in  place  to  determine  the  digitToRadDrive  constant. 
After  testing  each  wheel  for  a  minimum  of  1000  revolutions  (called  double  pi  or  DPI 
below)  the  digitToRadDrive  constant  was  determined: 

#define  digitToRadDrive  -6.015495746e-5 

/*  driving  constant  rad/count  =  DPI/1 04450  May  8  */ 


/*  Experimental  Results  by  Ed  Mays  May  7  */ 

/*  Wheel  1  count  =  104456  */ 

/*  Wheel  2  count  =  104435  */ 

/*  Wheel  3  count  =  104454  */ 

/*  Wheel  4  count  =  104455  */ 

/*  Average  count  =  104450  */ 

/*cf.  2048*51  =  104448  */ 
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Verification  of  the  constant  was  made  possible  by  the  engineering  data.  Given  the  range 
of  inputs  (same  as  Figure  3.12)  to  the  Servomotors  and  the  gear  ratio  (section  3.B.3)  the 
digitToRadDrive  was  be  verified  above  (cf.  2048  *  51  =  104448).  The  value  of  51  vice 
50  for  the  gear  ratio  (was  determined  empirically  to  be  1:51). 

The  digitToCmDrive  was  much  more  trivial  to  determine.  The  digitToRadDrive 
provides  the  number  of  counts,  knowing  the  wheel  radius  (18.9cm),  and  using  circle 
circumference  formula  yields: 

#define  digitToCmDrive  0.0011369287 

/*  driving  constant  cm/count  =  digitToRadDrive*l  8.9cm  Ed  Mays  */ 

/*  5/8/97  */ 

Now  having  the  “drive”  encoder  count  allows  the  computation  of  distance  traveled  (cm) 
by  the  vehicle;  and  coupled  with  the  timer  interrupt  allows  for  the  computation  of 
velocities  and  accelerations. 

2.  Measuring  Wheel  Speeds 

The  next  step  in  control  of  the  wheels  is  being  able  to  manipulate  wheel  speed. 
Developing  wheel  speed  control  was  one  of  the  projects  many  dead-end  path’s — that 
eventually  lead  to  great  results.  There  were  three  problems  with  the  work  presented  here. 
First,  the  inputs  or  digits  were  not  applied  at  the  lowest  (hardware)  level  using 
SpeedDigits  [Appendix  E],  instead  the  value  was  considered  desiredSpeeds.  Secondly, 
the  input  values  were  massaged  before  being  used  to  ensure  that  all  the  system  hardware 
would  accept  the  range  of  values.  Thirdly,  the  uniqueness  of  each  driving  motor  and 
natural  output  variance  over  a  range  of  inputs  was  not  considered  (e.g.,  used  averages 
instead  of  individual  motor  data).  How  did  this  happen?  A  minor  communication  error 
and  poor  naming  of  arrays  made  this  possible.  However,  a  look  at  the  results  and  the 
logic  for  deriving  them  will  provide  insight  towards  the  actual  solution. 

Include  are  an  estimated  velocity  and  the  software-measured  (actual)  velocity  as 
determined  by  software.  The  estimated  velocity  was  determined  by  applying  the 
requisite  input  and  measuring  the  number  of  seconds  it  took  for  wheel  one  to  complete  10 
revolutions  (e.g.,  wheel  radius  18.9cm,  distance  traveled  =  2  *  PI  *  Revolutions  *  radius 
=1187.5220  cm);  inaccuracies  from  this  measurement  came  from  hand  timing  and  the 
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version  of  pi  used  on  a  desk  calculator.  Velocity  calculated  from  V=distance  (cm)/time 
(seconds). 

The  software-measured  velocity  used  the  digitToCmDrive  constant,  which  has  the 
unit’s  cm/count.  The  algorithm  subtracts  the  previous  count  from  the  present  counter  (as 
read  from  the  counter  board)  and  multiplies  the  result  by  digitToCmDrive  leaving  the 
outcome  with  the  unit  of  cm.  This  outcome  is  then  divided  by  .01  (DeltaT)  representing 
100th  of  a  second  or  10  milliseconds  (corresponding  to  the  system  interrupt).  The 
99999999  represents  a  value  not  representable  by  the  counter  board.  The  output 
displayed  to  the  monitor  every  100  calls  of  the  routine  (mod  100).  An  example  of  the 
code  to  compute  each  wheel’s  driving  speed  can  be  seen  below: 

void  computeActualRatesO 

{ 

int  i; 

double  count, speed; 


for(i— 0;  i<— 3;  i++) 

{ 

if(PreviousCountSpeed|i]  ==  99999999)  /*  for  derivative  for  speed  */ 
Drivc_Speed_ Actual  fi]  =  0.0; 
else 

Drive_Speed_Actual  [i] = 

(convertDifference((WheelDriveValues[i]  -  PreviousCountSpeed[i])) 
*DigitToCmDrive[i])/DeltaT; 

PreviousCountSpeed[i]  =  WheelDriveValues[i]; 

} 
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Time 

Stop  watch 
(sec) 

Estimated  Velocity 
(cm/s) 

Software 

Measured  Velocity 
(cm/s,  average) 

10 

116.27  sec 

10.21349 

10.23235 

20 

58.15 

20.42170 

20.46.471 

30 

39.10 

30.37141 

30.35599 

50 

No  data 

No  data 

50.70702 

60 

No  data 

No  data 

60.93937 

70 

No  data 

70.94435 

80 

WKBSSBtM 

No  data 

81.29040 

90 

No  data 

No  data 

87.65720 

Figure  5.2:  Inputs  and  results  from  massaged  data  (error).  No  data  entries  exist  because  the 
revolutions  were  too  fast  for  hand  timing. 


If  an  input  of  100  is  used  the  estimated  values  for  velocity  are  nolonger  linear.  If  the 
inputs  vs.  velocities  were  linear  the  ratio  20/20.42170=100/X  used  to  predict  the 
estimated  velocity  would  yield  a  velocity  of  102.32355  cm/s;  however,  the  software- 
measured  velocity  was  87.54350  on  average  for  all  input  values  from  100  to  1000  (Figure 
5.3). 


:  1 

Time 

Stop  watch 
(sec) 

Estimated  Velocity 
(cm/s) 

Software 

Measured  Velocity 
(cm/s) 

No  data 

No  data 

87.65720 

No  data 

No  data 

87.65720 

400 

No  data 

No  data 

87.65720 

1000 

No  data 

No  data 

87.65720 

Figure  5.3:  Inputs  vs.  measured  velocity.  Figure  showing  a  velocity  saturation. 


Hence,  the  relationship  between  the  input  and  the  velocity  look  somewhat  linear  until  the 
area  where  the  input  is  greater  than  70--  after  this  velocity  saturation  seems  to  occur  (or 
maximums  reached).  Moreover,  if  one  closely  looks  at  the  slope  (i.e.,  y  =  mx  +  b) 
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between  the  input  ranges  20-30,  83-84,  and  87-88  it  is  clear  that  the  slope  changes 
dramatically  over  these  regions.  Even  though  the  wrong  input  structure  and  mechanism 
was  used  to  generate  this  data  a  lot  was  learned.  For  instance,  motor  performance  is  not 
completely  linear  and  each  motor  has  somewhat  unique  characteristics.  Once  the 
aforementioned  problems  were  identified  and  corrected  the  correct  data  [Appendix  N] 
could  be  plotted.  Figure  5.4  is  a  plot  of  the  correct  data. 

Driving  Velocity  vs.  Input 


saturation 


Figure  5.4:  Driving  Input  vs.  Velocity  plot.  Input  units  or  digits  are  written  directly  to  SpeedDigits. 
The  slope  changes  for  the  overall  graph  are  constant,  making  it  look  linear.  However,  over  some 
regions  this  graph  is  non-linear,  this  will  be  dealt  with  under  the  controls  section.  Velocities  also 
independently  verified  by  use  of  tachometer. 


3.  System  Controls 

So  far  controlling  the  speed  of  individual  servomotors  has  been  discussed. 
However,  it  is  known  individual  servomotors  provide  differing  outputs  for  the  same  input 
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ranges  in  some  cases.  Moreover  the  wheels  must  be  coordinated  and  work  together.  The 
goal  of  this  project  was  to  have  an  actual  driving  speed  that  has  less  than  1  %  error  when 
compared  to  the  desired  input  speed  (for  each  wheel).  How  is  this  close  tolerance 
accomplished?  This  small  margin  of  error  is  accomplished  by  using  well-established 
concepts  from  “control  systems”  theory.  A  general  control-system  structure  contains 
inputs  (or  reference  commands),  a  controller  (with  external  power),  control  forces,  a 
controlled  system  (plant),  disturbance  inputs,  outputs,  and  output  monitoring.  Control 
systems  are  almost  a  discipline  unto  themselves  requiring  knowledge  of  differential 
equations  and  Laplace  transforms.  Shepherd  is  looked  at  as  a  closed-loop  where  output 
monitoring  is  accomplished  through  sensors  (encoders)  and  the  information  passed 
through  feedback  channels.  The  feedback  results  in  a  closed  loop  signal  or  information 
flow.  The  controller  design  for  Shepherd  is  linear  and  considered  a  single-input-single¬ 
output  (SISO)  system.  Hence,  conceivably  the  state-variable  or  the  transfer-function 
(input-output)  method  could  be  used  here.  What  technique  did  we  use?  We  used  the  trial 
and  error  technique  [Ref.  17].  The  trial  and  error  technique  (Figure  5.5)  was  chosen 
because  of  the  skill  and  knowledge  levels  of  the  Shepherd  team.  The  advantages  of  this 
technique  are: 

1 .  Simple  mathematical  tools  are  used 

2.  Vast  amounts  of  experience  have  accumulated 

3.  Especially  well  adapted  for  use  with  computers 

4.  Linear  designs  usually  are  acceptable 

The  disadvantages  are: 

1 .  Inconsistent  performance  specifications  (PS)  can  be  encountered 

2.  Design  is  not  optimal 

3.  The  method  is  usually  suitable  mostly  for  SISO  systems. 
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Start 


Select  new 
PS 


Figure  5.5:  A  flow  graph  of  the  trial-and-error  design  process  [Ref.  18]. 

Using  the  experience  of  both  Professors  Kanayama  and  Yun  as  the  guide  the 
“black  box”  servo  structure  was  developed  (Figure  5.6).  The  servo  structure  is  called 
“black  box”  because  of  the  lack  of  understanding  of  the  servomotors  at  that  time 
(however,  inputs  and  outputs  could  be  measured).  The  previously  mentioned  PS  was  an 
error  rate  of  less  that  1%  of  the  given  reference  input.  There  were  other  PS’s  governed 
by  heuristics.  For  instance,  it  was  desired  that  the  actual  output  velocity  converges  (in 
the  mathematical  sense)  on  the  reference  or  input  velocity,  with  near  perfect  static  follow¬ 
up. 
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Figure  5.6:  “black  box”  servo  structure  with  feedforward  and  feedback  compensation. 

Vibrations  and  other  unacceptable  behaviors  were  used  to  determine  if  a  smooth 
and  acceptable  convergence  had  been  achieved  for  the  gain  used  (e.g.,  if  the  robot  was 
shaking,  this  was  not  acceptable).  An  acceptable  gain  would  be  one  that  produces  an 
oscillatory  response  that  converged  quickly  to  the  reference  input  (Figure  5.7).  Again 
this  same  type  information  could  have  been  determined  by  a  better  scientific  guess  [Ref. 
19]  using  a  closed-loop  differential  equation. 


Response  as  a  Function  of 


Figure  5.7:  Proposed  “proper”  oscillatory  response  as  a  function  of  loop  gain.  Response  determined 
for  Shepherd  using  experience  and  heuristics. 


43 


Previously,  it  was  stated  that  the  input  vs.  velocity  plot  was  not  “truly”  linear. 
These  non-linear  ranges  have  severe  physical  realities.  Also,  mentioned  was  the  point 
that  each  servomotor  produces  an  output  that  may  not  be  the  exact  same  as  the  outputs  of 
the  other  system  servomotors  given  the  same  input.  And  the  stated  PS  requires  that 
servomotor  outputs  be  within  1%  of  the  desired  input  or  input  speed.  This  translates  into 
several  challenges.  First,  using  the  feedforward  technique  constants  were  developed  to 
ensure  that  the  PS  of  1%  is  met.  The  following  algorithm  or  averaging  technique  was 
used  for  direct  testing  of  inputs: 

if  (vj  <  v  <  v2) 

d  =  di  +  (d2  -  di)/(v2  -vi)  *  (v  -  vi) 


The  v’s  and d’s  above  are  the  same  as  the  velocities  and  inputs  in  figure  5.8. 


Figure  5.8:  Dlustration  of  averaging  technique  used  to  select  “good” 
input  ranges. 

Using  the  aforementioned  averaging  technique  the  constants  were  developed  that 
maintained  all  servomotor  outputs  within  1%  PS. 
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These  constants  are  called  K1-K4,  and  K6.  These  constants  represent  the 
feedforward  values  that  are  applied  to  each  servomotor  over  a  specified  range  of  desired 
speeds.  The  desired  speed  ranges  give  are  because  of  the  piecewise  continuity  chosen 
because  of  the  non-linearity’s  in  the  input  vs.  velocity  plot  for  the  servomotors.  The  “K” 
constants  were  developed  with  no  load  on  the  system  (e.g.,  wheels  free  floating).  Listed 
below  are  the  constants  K1-K4,  and  K6  (speed  is  in  centimeters/second): 


Kl[0]=11.448;/*0<=speed<=5,*/ 
Kl[l]= 11.500; 

K1  [2] =11. 496; 

Kl[3]=12.375; 

K2  [0] = 1 1 . 500;/*5  >  speed<  8*/ 
K2[l]=11.500; 

K2  [2] =11.644; 

K2  [3] =12.000; 


K3[0]=11.611;/*8>=speed<20*/ 

K3[l]=l  1.585; 

K3[2]=l  1.686; 

K3[3]=l  1.840; 

K4[0]=11.711;/*20>=speed<=70*/ 

K4[l]= 11.659; 

K4[2]= 11.705; 

K4[3]  =  11.727; 

K6[0]  =  11.71 0;/*70>  speed<K5*/ 

K6[l]= 11.700; 

K6[2]=l  1.700; 

K6[3]= 11.715; 

Above  the  bracketed  values  are  array  element  numbers.  Element  [0]  refers  to 
wheel  1,  motor  Ml;  element  [1]  refers  to  wheel  2,  motor  M2;  element  [2]  refers  to  wheel 
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3,  motor  M3;  and  element  [3]  refers  to  wheel  4,  motor  M4.  A  constant  K5  was  also 
defined  as  87.4  cm/sec,  and  used  a  safety  cutoff  for  the  maximum  speed.  Figure  5.9 
shows  the  values  for  a  few  selected  desired  or  commanded  speeds  (most  are  within  the 
require  1%  error  in  PS). 


Ml  Speed 

M2  Speed 

M3  Speed 

M4  Speed 

10 

10.20 

10.08 

10.10 

9.89 

20 

20.12 

20.11 

20.11 

19.93 

30 

30.03 

30.00 

29.98 

40 

39.98 

39.91 

39.98 

50 

49.98 

49.79 

50.08 

60 

60.01 

59.77 

60.02 

60.02 

70 

69.91 

69.78 

69.95 

69.97 

Figure  5.9:  Desired  (commanded)  speeds  vs.  actual  “free  floating”  motor  speed.  The  application  of 
feedback  is  expected  to  move  the  speed  of  Ml  into  the  required  1  %  error  for  the  PS. 


At  this  point  all  the  tools  and  techniques  (i.e.,  trial-and-error,  scientific  method, 
and  the  experience  of  Professor’s  Kanayama  and  Yun)  are  in  place  for  the  application  of 
system  controls.  How  are  the  system  controls  (Figure  5.6)  implemented?  First,  the 
algorithm  is  presented: 

SpeedDigit  =  velocityReferenceTable(Omega_Speed,ix)  + 

DriveFeedBackGain*(Omega_Speed  -  Drive_Speed_Actual[ix]); 

SpeedDigit  represents  the  actual  digit  value  being  applied  to  the  servomotor.  In  the 
feedforward  part  of  the  loop  (Figure  5.6)  the  commanded  velocity  is  multiplied  with  a 
constant  Kl,  which  corresponds  to  the  “K"  constants  described  earlier  in  this  section?  In 
the  code  segment  above  the  function  “velocityReferenceTable”  is  called  (prior  to  the 
addition  of  DriveFeedBackGain*(Omega_Speed  -  Drive_Speed_Actual[ix])  ).  The 
“velocityReferenceTable”  applies  the  proper  “K”  constant  for  the  range  the  commanded 
speed  (Omega_Speed)  falls  within. 
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A  detailed  look  at  velocity  Ref erenceTable  is  provided  below: 


double  velocityReferenceTable(double  desiredVelocity,int  i) 

{ 

double  inVelocity, 
outVelocity; 

inVelocity=new_abs(desiredVelocity); 

if  (inVelocity>=0.0  &&  inVelocity<=5.0) 
outVelocity  =  inVelocity*Kl[i]; 

if  (inVelocity>5.0  &&  inVelocity<  8.0) 
outVelocity  =  inVelocity*K2[i]; 

if  (inVelocity>=:8.0  &&  inVelocity<20.0) 
outVelocity  =  inVelocity*K3[i]; 

if  (inVelocity>=20.0  &&  inVelocity<=  70.0) 
outVelocity  =  inVelocity*K4[I]; 

if  (inVelocity>70.0  &&  inVelocity<K5) 
outVelocity  =  inVelocity*K6[i]; 

if  (inVelocity>  K5) 

outVelocity = 1 023 ; 

if  (desiredVelocity<  0.0) 
outVelocity  =  -  outVelocity; 

return  outVelocity; 

}  /*  end  velocityLookupTablc  */ 
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It  should  be  noted  that  if  the  inVelocity  is  greater  than  K5,  then  the  outVelocity  is  given  a 
value  of  1023 — this  ensures  there  are  no  system  resets  because  the  input  digits  are  too 
large.  All  other  velocities  are  multiplied  by  a  specific  “K”  and  the  value  returned.  In  the 
feedback  part  of  the  loop  (Figure  5.6)  the  difference  between  the  commanded  velocity 
and  the  actual  velocity  is  multiplied  with  a  constant  DriveFeedBackGain  (also  K2  in 
Figure  5.6)  [Appendix  E,  lines  46  &  47].  Again  this 

(DriveFeedBackGain*(Omega_Speed  -  Drive_Speed_Actual[ix]) )  is  added  back  into  the 
inputs  used  for  the  next  time  the  process  is  run  (based  on  the  10  millisecond  timer 
interrupt). 

Hence,  now  the  final  key  to  this  control  system  would  be  finding  a 
DriveFeedBackGain  constant  that  would  provide  the  desired  “proper”  oscillatory 
response  (Figure  5.7)  and  ensure  the  servomotor  output  velocity  is  within  the  1%  error  of 
the  commanded  velocity  required  by  the  PS.  Using  Occam’s  Razor,  the  trial-and-error 
flow  diagram  (Figure  5.5),  the  iterative  approach  in  the  scientific  method  (Figure  5.1), 
and  the  heuristics  provided  by  Professor’s  Kanayama  and  Yun,  the  search  for  the  best 
DriveFeedBackGain  was  initiated.  The  heuristics  used  were  observation  based.  First, 
the  “gain”  used  could  not  cause  the  vehicle  to  shake  in  any  visible  manner.  Secondly,  the 
“gain”  used  had  to  quickly  move  the  actual  servomotor  speed  to  the  commanded  speed  if 
there  was  a  difference.  Pseudo  random  values  were  chosen  as  gains,  based  on  the 
experience  of  the  aforementioned  professors.  The  real  number  range  [-1.0,  1.0]  was  used 
to  test  the  gains.  On  the  negative  end  of  the  range  the  gain  was  incremented  by  +.05, 
until  the  gain  equaled  zero — the  results  were  not  acceptable.  On  the  positive  end  of  the 
range  the  gain  was  decremented  by  +.05,  until  the  gain  equaled  zero — at  .8  the  gain 
showed  the  best  results  (i.e.,  range  [0.0,  1.0]).  The  gain  was  defined  as 
DriveFeedBackGain  =  .8  [Appendix  J,  Consolidated  header  files,  line  389].  Hence  the 
gain  met  all  the  criteria  for  an  acceptable  design  (Figure  5.5)  and  validates  the  “black 
box”  servo  structure  as  envision  by  professors  Kanayama  and  Yun  (Figure  5.6). 
Moreover,  the  experiment  can  be  considered  a  multidisciplinary  success  between  physics, 
electrical  engineering,  and  computer  science. 
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c. 


WHEEL  STEERING 


As  with  wheel  driving,  the  same  approach  to  wheel  steering  was  used  in 
measuring  the  counts  from  the  counter  board  and  measuring  the  rate  of  turning  of  each 
wheel  over  time.  The  goal  was  to  observe  and  measure  each  wheels  turn  rate  and  create  a 
feedback  compensation  loop  as  in  wheel  driving  to  ensure  that  the  PS  of  1%  is  met. 


1.  Developing  Steering  Constants 

The  steering  constants  were  developed  in  the  same  way  as  the  driving  constants. 
The  counter  board  was  read  for  steering  values  and  then  displayed  .  A  digitToRadSteer 
(input  digits  per  radians  for  steering)  constant  value  of  -6.817692391e-5  (rad/count  = 
(2*pi)/(2048*45))  and  RadRateTodigit  constant  value  of  195.4155  (digits/rad/sec  = 
1023/5.23598)  was  determined  by  observation  of  the  data  forthcoming. 


2.  Measuring  Wheel  Rate  of  Turn 

In  measuring  the  wheel  rate  of  turn,  the  same  approach  was  taken  as  discussed 
before.  An  estimated  rate  of  turn  and  the  software-measured  (actual)  rate  of  turn  are 
included.  The  estimated  rate  was  also  determined  by  applying  an  input  and  measuring 
the  number  of  seconds  it  took  for  a  wheel  to  completely  rotate  360  degrees.  As  can  be 
deduced,  a  certain  amount  of  error  was  introduced  due  to  human  timing  interaction. 


Desired  rate  of 

turn 

(rad/s) 

Time 

Stop  watch 
(sec) 

Estimated  Rate 

(rad/s) 

Software 

Measured  Rate 

(rad/s,  average) 

1 

6.0 

1.00000 

0.98174 

2 

3.5 

1.79485 

1.95667 

3 

2.19 

2.86849 

2.93160 

5 

1.69 

2.71716 

3.90653 

5.5 

No  data 

No  data 

4.88828 
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Desired  rate  of 

turn 

(rad/s) 

Time 

Stop  watch 
(sec) 

Estimated  Rate 

(rad/s) 

Software 

Measured  Rate 

(rad/s,  average) 

10 

No  data 

No  data 

5.23598 

20 

No  data 

No  data 

5.23598 

30 

No  data 

No  data 

5.23598 

Figure  5.10:  Inputs  and  results  from  massaged  data  (error).  No  data  entries  exist  because  the 
revolutions  were  too  fast  for  hand  timing. 


By  observing  the  data  from  the  above  figure  we  can  confirm  that  a  certain  amount 
of  error  is  inherent  in  the  system.  As  the  input  value  increases  from  0  to  5.5  radians  per 
second  ,  a  linear  correspondence  tended  to  exist.  However,  after  an  input  of  5.5  rad/s  the 
software  measured  average  tended  to  be  5.23598  rad/s  resulting  in  a  saturation  state.  This 
result  in  itself  could  not  give  us  the  exact  range  of  values  where  this  occurred.  Therefore, 
manipulation  of  the  steerDigits  was  necessary.  Appendix  O  contains  these  results  given 
in  inputs  of  digits  vice  desired  rate  of  turn.  At  the  maximum  input  speedDigit  of  1023 
saturation  is  reached.  The  wheels  will  only  turn  at  an  average  rate  of  5.235  radians  per 
second.  Thus  the  software  measured  average  of  5.23598  rad/s  was  adopted. 


3.  Steering  Feedback 

By  further  observation  of  the  data  obtained,  an  average  was  used  in  obtaining  the 
rate  of  turn.  Not  all  the  wheels  turn  at  the  same  rate.  The  objective  was  to  have  a  less 
than  1%  PS  for  optimization.  To  achieve  that  another  “black  box”  servo  structure  was 
developed  .  The  Figure  5.1 1  is  a  pictorial  representation  of  this  and  is  a  little  different 
than  that  of  wheel  driving. 
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Figure  5.11:  ‘‘black  box”  servo  structure  with  series  and  feedback  compensation  for  wheel  steering 
feedback  control. 


In  finding  the  K  constants  from  the  figure,  trial  and  error  is  also  used.  The 
representation  of  the  algorithm  in  code  is  presented: 

Steer_Digit  =  rateReferenceTable(desiredAngleRates[ix]) 

+  steerFeedbackGain*(desiredAngleRates[ix]-actualAngleRates[ix]) 

+  angleFeedbackGain*norm(desiredAngles[ix]-actualAngles[ix]); 

Steer_Digit  represents  the  actual  digit  value  being  applied  to  the  servomotor.  The 
rateReferenceTable  function  simply  converts  the  inputed  rate  to  digits  or,  in  the  case 
where  the  inputed  rate  is  larger  than  5.235,  clips  it  to  within  limits.  The  function  is  given 
below: 


double  rateReferenceTable(double  desiredRate) 

{ 

double  inRate, 
outDigit; 

inRate=new_abs(desiredRate); 

if  (inRate<=  5.234) 
outDigit  =  inRate*  1 95.41 55  ; 
else 

outDigit=  1 023; 
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if  (desiredRate<  0.0) 
outDigit  =  -  outDigit; 

return  outDigit; 

} 

The  two  K  constants  steerFeedbackGain  and  angleFeedbackGain  were  determined 
by  trial  and  error.  AngleFeedbackGain  was  first  maintained  at  0.0  while  testing 
steerFeedbackGain.  SteerFeedbackGain  was  made  very  low  at  the  outset  and  increased 
through  each  test  until  the  vehicle  displayed  unusual  behavior  such  as  shaking  while 
operating  its  steering  function.  At  this  point  the  value  was  lowered  and  then  the  value  for 
angleFeedbackGain  was  increased  in  the  same  way.  These  values  became  optimal  at 
100.0  and  1000.0  for  steerFeedbackGain  and  angleFeedbackGain  respectively. 


4.  Wheel  Testing 

It  was  discovered  while  operating  the  vehicle  that  wheel  4  would  on  occasion  be 
very  badly  misaligned  from  the  other  3  wheels.  Even  after  repeated  realignment  it  would 
not  operate  as  the  others.  At  suspect  was  the  thought  this  problem  was  software  related  . 
To  test  this,  a  routine  was  inserted  into  the  SRK  driveMotors()  function  with  a  menu  item 
on  the  user  interface.  This  routine  simply  turned  wheel  4  360  degrees  in  one  direction 
until  the  wheel  aligned  read  the  encoders  for  angle  position,  paused  one  second  and  then 
turned  it  in  the  opposite  direction.  At  each  pause,  the  wheel  position  was  displayed  to  the 
interface  screen  and  recorded.  The  data  obtained  is  presented  below  in  Figure  5.12  for  10 
iterations  for  clockwise  and  counterclockwise  rotation: 


Clockwise  Rotation 

Counterclockwise  Rotation 

1 

000.867 

360.390 

2 

000.878 

360.390 

3 

000.976 

360.363 

4 

000.933 

360.414 

5 

000.984 

360.371 

6 

000.992 

360.394 

7 

000.992 

360.453 
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Clockwise  Rotation 

Counterclockwise  Rotation 

8 

000.902 

360.394 

9 

000.996 

360.445 

10 

000.996 

360.476 

Figure  5.12:  Wheel  4  data  based  on  position  of  rest  after  direction  of  turn. 


The  average  values  for  rotation  in  both  the  clockwise  and  counter  clockwise  directions 
were  000.955  and  360.426.  These  were  averaged  for  20  iterations  see  Appendix  O  for 
the  full  data  set.  Even  though  this  data  was  obtained  while  the  wheel  was  in  a  free 
floating  environment  without  added  friction,  it  still  proved  that  wheel  4  was  operating 
within  1%  PS.  Therefore,  from  this  result  a  conclusion  is  drawn  that  the  problem  is  not 
software  related  in  nature  but  maybe  mechanical. 
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VI.  MOTION  MODES 


A.  OVERVIEW 

Chapter  II  mentioned  several  modes  of  motions  that  can  be  exhibited  by 
Shepherd.  Due  to  the  nature  of  Shepherd’s  characteristics  of  having  four  wheels  that  can 
be  independently  operated  with  two  degrees  of  freedom,  this  makes  it  possible  to  obtain 
three  degrees  of  freedom  motion.  In  this  chapter  we  will  discuss  the  modes  of  “Tornado” 
(complex  motion),  Joystick  controlled  motion  and  searching  motion  simulation.  The 
other  motion  modes  are  encompassed  by  these  motions.  The  code  for  the  other  motions 
are  provided  in  the  Appendices.  Also,  the  Tangential  mode  will  not  be  discussed  but  will 
be  left  for  a  future  reasearch  topic  and  implementation.  However  the  ground  work  as 
well  as  the  code  is  in  place  for  implementation  (Appendix  D). 

Before  embarking  on  this  discussion  on  motion  modes  the  theory  or  basis  for 
motion  control  must  be  presented  This  control  motion  theory,  as  well  as  the  figures  to 
follow,  was  taken  from  the  works  of  Professors  Yutaka  Kanayama  and  Xiaoping  Yun 
[refs  23  &  24],  First  a  vehicle  coordinate  system  is  defined  on  a  rigid  body  robot.  A 
configuration  q  is  defined  as 

(p,  y)  =  ((x,  y),  \|/), 

where  p  is  the  positioning  of  the  vehicle  origin  and  psi  is  the  heading  orientation  of  the 
vehicle  Xv-axis.  Next  in  describing  the  motion  of  the  vehicle’s  configuration  which  is  a 
function  of  time,  the  following  is  the  definition: 

q(t)  =  (p(t),  \|/(t»  =  ((x(t),  y(t»,  i|/(t)), 

where  p(t)  is  the  translational  component  and  V|/(t)  is  the  rotational  component  of  the 
vehicle  motion.  Figure  6.1  is  an  illustration  of  this  configuration  and  motion.  Because 
the  vehicle  possesses  2  dimensional  positioning  it  can  exhibit  3  degrees  of  freedom 
motion.  This  motion  contains  three  variables  of : 

•  Translation  speed  —  v(t)  =  V  ((dx(t)/dt)2  +  (dy(t)/dt)2), 


54 


•  Motion  direction  —  0(t)  =  atan2(dy(t)/dt,  dx(t)/dt),  if  v(t)  >  0, 

•  Rotational  speed  -  co(t)  =  d\(/(t)/dt. 

Resulting  in  a  motion  description  of: 

Q(t)  =  (v(t),  0(t),  ©(t)). 


Figure  6.1:  Configuration  and  Motion  of  a  Rigid  Body  [Ref.  23]. 

As  was  mentioned  in  Chapter  II,  3  typical  vehicle  motions  are  possible.  The  3 
typical  motions  are  illustrated  in  Figure  6.2.  Part  (a)  represents  an  all  too  familiar  motion 
exhibited  by  a  normal  automobile  or  bicycle  and  is  referred  to  as  tangential  motion.  This 
is  characterized  by  the  fact  that  the  vehicles  heading  orientation  is  equal  to  its 
translational  motion  direction  (  di|/(t)/dt  =  0(t)/d(t)).  Part  (b)  depicts  a  motion  called 
constant  orientation  where  the  vehicles  heading  orientation  is  constant  or  rotational  speed 
of  the  vehicle  is  0.  And  part  (c)  shows  a  complex  motion  which  incorporates  rotation 
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Figure  6.2:  Typical  vehicle  motions  [Ref.  23]. 


independently  superimposed  on  a  translational  motion.  This  complex  motion  is  the  basis 
for  the  “Tornado”  motion  mode  to  be  discussed. 


B.  “TORNADO”  MOTION 

In  analyzing  this  motion  mode,  several  more  discriptions  will  have  to  be  made 
clear.  One  is  that  of  a  point.  A  point  is  defined  as 


pi  =  (xi,yi)*(0,0) 


described  in  the  vehicle  coordinate  system.  On  the  rotary  vehicle  it  corresponds  to  a 
wheel .  In  the  case  of  wheels  1-4,  it  would  be  (40,-40),  (40,40),  (-40,  -40),  and  (-40,40). 
So  we  will  have  to  evaluate  how  these  wheels  move  while  the  vehicle  is  executing  the 
input  motion  or  Q.  In  order  to  evaluate  this,  the  polar  coordinate  representation  is 
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another  description  that  is  needed.  This  representation  is  defined  as  (p,  a)  and  is 
represented  as 

p  =  V(xi2  +yi2)  and  a  =  atan2(yi,  xi). 

The  subscript  is  a  representation  of  the  wheel  number  and  can  represent  any  wheel  based 
on  the  wheel  location  in  the  vehicle  coordinate  system.  Figure  6.3  is  a  representation  of 
the  composite  motion  of  a  point  on  a  vehicle. 

e 


Figure  6.3:  Composite  Motion  of  a  Point  on  a  Vehicle  [Ref.  231. 
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Given  the  above,  a  current  configuration  and  motion  of 


q(t)  =  ((x(t),  y(t)),  \)/(t)), 

Q(t)  =  (v(t),  6(t),  co(t)), 

in  the  global  coordinate  system,  the  x  and  y-components  of  vlx(t)  and  viy(t)  in  the  global 
coordinate  system  can  be  determined  mathematically  [Ref.  23  p.3].  The  motion  speed 
vi(t)  and  direction  0i(t)  in  the  global  coordinate  system,  motion  direction  0]V(t)  in  the 
vehicle  coordinate  system,  and  rotation  rate  ff»iV  of  pi  or  any  wheel  is: 

V,(t)  =  V(  (vlx(t))2  +  (vly(t))2) 

0,(t)  =  atan2(vJy(t),  vlx(t)) 

0,v(t)  =  01(t)-\|/(t) 

to,v  =  ((v2-O-+p2co3)+vpco(a)+'O-)sin(0-\(/  a)+p(vn-*©)cos((0-\|/-a))/vi2  -  co 
-0-  is  theta  dot,  D  is  omega  dot,  and  ♦♦♦  is  motion  speed  dot. 

Corresponding  to  the  above  equations  in  SRK  is  the  following  code  from  Appendix  D 
(movement.c): 


ro=whp[i].rho; 

ro2=ro*ro; 

beta=vehicle.heading+whp  [i]  .alpha; 
vlx  =  speed*cos(theta)-(whp|I].rho*omega*sin(beta)); 
vly  =  speed*sin  (theta) +(whp[i].rho*omega*cos  (beta)); 
desiredSpeeds[i]  =  new_sqrt(vlx*vlx  +  vly*vly); 

if  (new_abs(desiredSpeeds[i])  >  0.01)  { 
desiredAnglesfi]  =  atan2(vly,vlx)  -  vehicle.heading; 
wheelAngleV  =  motion.Theta  -  vehicle.heading  -  whp[i]. alpha; 
desiredAngleRatcs[i]  =  ( (spced+ speed* thetaDot  4-  ro2*Omega3) 
+speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 
/( desiredSpeedsfi]*  desiredSpeedsfij)  -  omega; 
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desiredAnglesO[i]  —  desiredAnglesfi]; 
desiredAngleRatesO[i]  =  desiredAngleRates[i]; 

} 

The  above  code  is  very  straight  forward.  A  direct  correlation  can  be  discerned 
from  the  theory  to  the  implementation.  The  variables  were  named  as  closely  as  possible 
to  match  the  theory  presented.  The  resulting  code  provided  the  mathematical 
computation  providing  the  resulting  values  from  a  rotation  superimposed  independently 
on  a  translation  motion.  It  is  not  shown,  but  all  variables  with  brakets  enclosing  an  ‘i’ 
represent  a  wheel.  The  entire  routine  is  enclosed  in  a  for  loop  which  is  iterated  four 
times.  Therefore,  a  resulting  value  is  computed  and  provide  to  each  wheel’s  servo  for 
both  driving  and  steering  . 

A  detailed  proof  of  the  rotation  rate  of  the  moving  direction  at  a  point  can  be 
found  in  Ref.  24.  As  was  stated  earlier,  this  motion  control  theory  is  from  the  work  of 
Professors  Kanayama  and  Yun.  The  authors  simply  implemented  this  theory  in  code  and 
applied  it  to  the  rotary  vehicle. 


C.  JOYSTICK  CONTROLLED  MOTION 

As  discussed  in  chapter  IV  the  user  interface  is  provided  by  a  laptop  that  includes 
a  selection  menu  of  shepherd  functions.  There  are  many  options  (still  expanding,  a  work 
in  progress)  on  the  menu.  The  two  of  concern  here  are  options  three  (3)  and  four  (4), 
straight  motion  by  joystick  and  XY-motion  by  joystick.  Actually  the  emphasis  will  be  on 
option  four  because  option  three  can  be  considered  a  logical  subset  of  option  four. 

The  “driver”  function  discussed  in  the  file  movement.c  is  called  every  10 
milliseconds.  One  of  the  key  functions  executed  under  driver  is  the  call  to  another 
function  named  “bodyMotion”.  The  bodyMotion  function  is  also  located  in  movement.c. 
When  a  user  selects  option  three  or  four  from  the  menu  a  motion  mode  3  or  4  is  chosen. 

If  the  user  chooses  option  four  the  motion  mode  is  4.  The  case  the  user  chose  causes  the 
joystick  to  be  read  by  the  readJoyStick  function  (Appendix  H,  utils.c).  This  function 
reads  the  three  ports  (A,  B  and  C)  from  the  Intel  85C55  Parallel  Port  1  (Taurus  board) 
and  converts  them  into  an  ASCII  string  (code  segment  below): 
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void  readJoyStick(void) 

{ 

unsigned  int  i,index; 

unsigned  char  *ctrlPort=  (unsigned  char*)PI01_CTRL; 
unsigned  char  *dataPort=  (unsigned  char*)PI01_DATA; 
unsigned  int  pioPortl  [3]; 
double  a=  0.1,  xx,  yy,  zz; 

*ctrlPort=0x9b;  /*  set  all  ports  (A,B,C)  into  input  mode  (read  only)  */ 
index=10;  /*  position  for  x-digits  in  string  JOYSTICK  */ 

for  (i=0;i<3;i++) 
pioPortl  [i]  =  *(dataPort+i); 

xx  =  (double)pioPortl[0]- 128.0; 
yy  =  (double)pioPortl[l]-128.0; 
if  (xx  >=  0.0) 
xx  =  xx*xx/100; 
else 

xx  —  -xx*xx/100; 
if  (yy  >=  0.0) 
yy  -  yy*yy/i00; 
else 

yy  =  -yj^yy/ioo; 

joyStick.x  =  a*(xx)  +  (1.0-a)*joyStick.x; 
joyStick.y  =  a*(yy)  +  (1.0-a)*joyStick.y; 

if  (pioPortl  [2] ==0x03) 

setVME((unsigned  char  *)VME92 10,0x00);  /*  no  button  pressed  */ 
else  { 
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setVME((unsigned  char  *) VME92 1 0,0x02) ;  /*  if  any  button  pressed  */ 

} 

} 

It  should  be  noted  that  the  joystick  input  integer  range  is  from  [-127,128];  the 
intersection  of  the  ‘x’  and  ‘y’  axis  on  the  physical  joystick  defines  the  center  (x=0,  y=0). 
Once  the  port  is  read  some  data  smoothing  is  done.  Due  to  the  sensitive  nature  of  the 
inputs  a  parabolic  function  was  added  for  control  (this  can  be  seen  above  with  the 
manipulation  of  the  xx  and  yy  variable).  The  purpose  of  the  parabolic  function  is  to 
ensure  that  when  the  joystick  input  values  are  small  (near  zero,  center  on  the  physical 
joystick)  the  slope  changes  will  be  of  minimal  effect,  however  if  the  input  values  are 
large  (away  from  the  physical  joystick  center)  the  effect  on  velocity  or  steering  will  also 
be  proportionally  large.  The  smoothing  is  continued  for  because  of  the  possibility  of  very 
quick  slope  changes  in  the  data  being  read-in.  The  objects  joyStick.x  and  joyStick.y 
receive  values  that  are  only  10  percent  (a  =  0.1  in  the  code  segment)  of  the  xx  or  yy  value 
plus  90  percent  of  the  previous  value  for  xx  or  yy.  The  aforementioned  smoothing 
techniques  were  developed  based  on  the  experiences  of  Professor  Kanayama  an  the 
constant  “a  =  0.1”  determined  by  testing  for  the  “best”  hand  feel  and  response. 

Upon  completion  of  the  read  and  smoothing  of  the  joystick  data,  these  values  (i.e., 
speed  and  theta)  are  passed  to  the  wheelMotion  function  described  in  the  Shepherd 
Motion  Control  Architecture  (see  code  segment  below): 

case  4:  /*  X-Y  Morion  by  Joystick  */ 

readJoyStickO;  /*  ejm  19  july  97*/ 

speed  —  -joyStick.y*0.1;  /*  speed  control,  0.1  determined  by  testing  */ 
theta  =  -joyStick.x*0.02;  /*  steering  control,  0.02  determined  by  testing  */ 
if  (theta  >  HPI)  theta  =  HPI; 
if  (theta  <  -HPI)  theta  = -HPI; 

/*  omega  =  -joyStick.omega*0.1;*/  /*  pending  ejm  24  july  97  */ 
break; 
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The  two  “if’  a  statements  with  the  theta  conditions  above  reflect  the  capability  of 
the  rotary  vehicle  to  complete  perpendicular  driving  and  parking.  Actually,  the  theta 
values  (steering  angle)  of  the  Shepherd  vehicle  are  unlimited,  however  they  are 
constrained  here  for  ease  of  use  and  control. 

D.  SEARCHING  MOTION 

The  searching  motion  discussed  here  is  based  on  the  requirement  to  have  a 
smooth  technique  that  allows  shepherd  to  evenly  and  precisely  search  an  area  for  UXOs. 
The  aforementioned  search  algorithm  and  its  implementation  are  not  trivial.  The 
algorithm  and  simulation  presented  here  are  the  results  of  a  lifetime  of  work  by  Professor 
Kanayama.  Professor  Kanayama’s  expertise  in  the  areas  of  motion  planning,  motion 
design,  vehicle  kinematics,  sensing,  guidance,  learning,  environmental  representation, 
and  control  architectures  for  autonomous  vehicles  was  the  major  influence.  Professor 
Kanayama’s  work  on  the  Yamabico-11  robot  includes  development  of  composite 
function,  line  tracking,  circle  tracking,  and  neutral  switching  technique  [Ref.  7].  For  the 
aforementioned  search  algorithm  and  simulation  the  composite  function  and  line  tracking 
technique  will  be  used. 

The  goal  of  this  simulation  is  to  show  that  if  given  an  orientation  for  the  vehicle 
body  and  a  given  path,  that  the  path  can  be  tracked  smoothly  and  the  vehicle  orientation 
will  also  change  to  ensure  area  coverage  of  the  path  traveled.  Why  is  this  important? 
This  is  critical  because  the  desire  is  for  the  vehicle  to  search  the  path  in  the  most  safe, 
smooth,  and  efficient  manner.  The  first  assumption  is  that  the  time  required  to  move 
across  a  path  is  10  milliseconds  or  0.01  seconds.  Secondly,  an  assumption  can  be  made 
for  the  vehicle  orientation  (called  psi  here),  psi  starts  at  3*PI/4.0.  Along  the  path  traveled 
the  orientation  or  psi  will  move  from  3*PI/4.0  to  PI/4.0  (having  a  net  change  of  PI/2.0). 
From  the  aforementioned  change  in  psi,  the  incremental  change  over  time  can  be 
determined  (this  incremental  change  is  called  omega).  Dividing  the  net  difference  in  psi 
(Pi/2.0)  derived  omega  by  10  milliseconds,  resulting  in  an  omega  value  of  0.1570796327. 
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A  value  of  40  centimeters  per  second  was  arbitrarily  chosen  for  the  vehicle  velocity.  For 
the  simulation  the  initial  vehicle  body  coordinates  as  x  =  0,  y  =  0,  and  the  vehicle 
orientation  as  shown  above  3*PI/4.0.  Also,  the  coordinates  of  the  wheels  must  be  known. 
Hence,  we  place  the  vehicle  wheels  on  sides  that  are  80  centimeters  in  length  (like  the 
Shepherd  vehicle). 
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The  last  item  is  the  structure  required  supporting  the  simulation  [Ref.  M],  below 
is  a  code  segment  to  illustrate  the  aforementioned  statements. 

double  deltaTime  =  0.01; 

double  Vel  =  40.0; 

double  omega  =  -0.1570796327; 

typedef  struct  { 
double  x; 
double  y;  } 

POINT; 

typedef  struct  { 

POINT  Point; 
double  Theta; 
double  Kappa; 
double  Psi; 

} 

CONFIGURATION; 

q_init.Point.x  =  0.0; 
q_init.Point.y  =  0.0; 
q_init.Theta  =  0.0; 
q_init.Kappa  =  0.0; 

q_init.Psi  =  2.356219449;  /*  3*PI/4.0  */ 
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/ /individual  wheel  coordinates 
qfrontR.Point.x  =  40;  /*  wheell  */ 
qfrontR.Point.y  =  -40; 

qfrontL.Point.x  =  40;  /*  wheel2  */ 
qfrontL.Point.y  =  40; 

qrearR.Point.x  =  -40;  /*  wheeB  */ 
qrearR.Point.y  =  -40; 

qrearL.Point.x  —  -40;  /*  wheel  4  */ 
qrearL.Point.y  =  40; 


q_xaxis.Point.x  =  0.0;  /*  line  to  be  tracked,  initial  configuration  */ 
q_xaxis.Point.y  =  40.0; 
q_xaxis.Theta  =  0.0; 
qjxaxis.Kappa  =  0.0; 

Another  key  element  in  the  structure  CONFIGURATION  is  theta.  Theta  is 
simply  the  angle  to  the  path  being  tracked.  For  instance  if  the  vehicle  is  tracking  a  line, 
when  the  vehicle  move  onto  the  actual  line  then  Theta’s  value  goes  to  zero.  The  final 
element  required  for  the  simulation  is  the  step  size  that  is  used  for  the  motion.  The  step 
size  in  the  simulation  code  is  called  deltas,  and  is  vel*deltaTime  (or  .4  centimeters  per 
second).  Armed  with  this  knowledge  a  simplified  discussion  can  take  place  (for  detailed 
knowledge  of  the  compose  function,  and  line  tracking  see  Professor  Kanayama’s  motion 
planning  and  kinematics  notes  [Ref.  7]). 
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The  compose  function  is  used  to  determine  (using  deltas  )  the  next  position  of  the 
vehicle  body.  Here  two  compose  functions  are  being  used,  one  that  is  for  the  wheels 
(Compose2)  and  another  for  the  vehicle  body  (Compose). 

CONFIGURATION  Compose(CONFIGURATION&  ql,CONFIGURATION& 
q2,  CONFIGURATION&  q3,  double&  s,  double&  deltaTime) 

{  double  x,y, 

sinTheta  =  sin(ql  .Theta), 
cosTheta  =  cos  (ql  .Theta); 

x  =  ql  .Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*sinTheta; 
y  =  ql. Pointy  +  q2.Point.x*sinTheta  +  q2.Point.y*cosTheta; 
q3.Point.x  =  x; 
q3.Point.y  =  y; 

q3.Theta  =  ql  .Theta  +  q2.Theta; 

q3.Psi  =  ql.Psi  +  (omega  *  deltaTime);  /*  how  to  handle  move  left/right?  */ 
fprintf(f6,"%l  0.3f  %1 0.3f  %1 0.3f  %1 0.3f  %1 0.3  An", 
s,q3.Point.x,  q3.Point.y,q3.Theta,  q3.Psi); 
return  q3; 

}//  end  Compose 


CONFIGURATION  Compose2(CONFIGURATION&  ql,CONFIGURATION& 
q2,  CONFIGURATION&  q3)  /^position  */ 

{  double  x,y, 

sinTheta  =  sin(ql.Psi), 
cosTheta  =  cos(ql.Psi); 

x  =  ql. Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*sinTheta; 
y  =  ql. Point.y  +  q2.Point.x*sinTheta  +  q2.Point.v*cosTheta; 
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q3.Point.x  =  x; 
q3.Point.y  =  y; 
return  q3; 

}  //  end  Compose2 

The  Compose  function  contains  several  lines  that  are  important  for  observations 
in  our  simulation.  The  calculation  of  q3  .Theta,  as  mentioned  before  the  data  theta  should 
grow  in  positive  manner  as  the  vehicle  moves  from  its  initial  point  to  the  line  above  it. 
Once  the  line  that  is  being  tracked  has  been  reached  the  value  for  theta  goes  to  zero. 
Secondly  the  calculation  for  psi  shows  that  as  the  vehicle  moves  from  the  initial  position 
to  the  end  of  the  line  being  tracked,  the  value  of  psi  will  be  decremented  by 
omega* deltatime  (note  in  the  code  omega  is  defined  as  a  negative  number  [Appendix 
M]).  The  Compose2  function  is  important  because  it  provides  the  ability  to  compose  the 
body  orientation  (psi)  with  the  x  and  y  cooridinates  using  the  previously  defined  step. 

In  the  actual  simulation  the  values  of  theta  were  manipulated  to  ensure  the  vehicle 
would  track  the  next  line  above  (40  centimeters  higher)  on  the  next  step  through  the  loop 
(see  the  code  segment  below): 

q_xaxis.Point.y  =  q_xaxis.Point.y  +  40.0; 
if(ix%2==0)  { 
q_xaxis.Theta  =  PI; 
q.Theta  =  PI; 
omega  =  fabs(omega); 

}else{ 

q_xaxis.Theta  -  0.0; 
q.Theta  =  0.0; 
omega  =  -omega; 

The  simulation  proved  successful  based  on  the  data  provided  in  Appendix  L.  Figure  6.4 
is  a  graphical  representation  of  the  simulation  data  in  Appendix  L.  Moreover,  all  the 
structures  are  in  place  in  the  SRK  [Appendix  J,  Consolidated  header  files]  to  implement 
the  sensing  motion.  If  more  time  was  available  for  this  thesis  the  sensing  motion  would 
have  been  implemented. 
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VII.  CONCLUSIONS 


A.  SUMMARY 

What  was  accomplished  as  a  result  of  this  research.  A  restatement  of  the  goals  or 
questions  addressed  of  this  is  necessary.  The  thesis  was  to  examine  the  following 
research  areas: 

•  What  kinematics  algorithms  must  be  developed  to  support  a  vehicle  with  three 
degrees  of  freedom  of  motion?  The  aforementioned  algorithms  must  support 
highly  flexible,  controlled,  and  precise  motion. 

•  What  types  of  controls  are  required  to  ensure  the  optimal  mix  of  driving  and 
steering  resources?  Moreover,  what  must  be  done  to  ensure  that  all  the 
resources  complement? 

•  How  can  the  knowledge  gained  in  the  aforementioned  research  areas  be  used 
to  develop  searching  motion? 

•  How  should  the  hardware  and  software  systems  be  implemented  to  support 
the  aforementioned  goals? 

The  kinematics  algorithms  developed  to  support  a  vehicle  with  3  degrees  of 
freedom  of  motion  can  be  divided  into  2  categories,  ’low  level'  and  'high  level'.  The  first 
category  is  'low  level'.  The  'low  level’  algorithms  are  illustrated  in  Chapter  IV  and  in 
Appendix  E  (motor.c).  These  'low  level'  algorithms  are  concerned  with  taking  inputs  and 
passing  the  inputs  to  the  hardware.  The  empirical  results  from  the  direct  input  and 
servomotor  output  can  be  seen  in  Chapter  V.  The  'high  level'  category  considers  desired 
body  motion  and  its  transformation  into  wheel  motion  (see  Chapter  IV  and  Appendix  D). 
The  driver  function  in  movement.c  binds  the  high  and  low  level  categories  allowing  for 
highly  flexible,  precise,  and  controlled  motion  of  the  rotary  vehicle. 

The  controls  required  for  the  optimal  mix  of  driving  and  steering  resources  are 
developed  in  Chapter  V.  The  algorithms  and  their  implementation  as  described  in 
Chapter  V  removes  or  lessens  the  effect  of  variance  and  disparities  of  servomotor  output. 
This  ensures  an  optimal  mix  of  driving  and  steering  resources  and  that  resources  are 
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utilized  in  a  complimentary  manner.  Hence,  the  aforementioned  algorithms  gives  the 
desired  output  within  1%  error  for  performance  specifications. 

The  knowledge  gained  and  the  researched  discribed  in  Chapters  IV,  V,  and  VI  can 
be  used  to  implement  the  required  searching  motion  the  rotary  vehicle  needs  for  the  UXO 
effort.  Discoveries  and  knowledge  gained  during  development  of  the  Tornado, 
Sinusoidal,  and  Joystick  controlled  motions;  coupled  with  the  searching  motion 
simulation  can  be  used  to  implement  the  tangential  and  searching  motion  (see 
Appendices  C  and  D).  Moreover,  the  structures  are  presently  in  place  to  support  both 
tangential  and  searching  motions  in  Appendix  D. 

The  hardware  and  software  systems  are  implemented  as  described  in  Chapters  HI 
and  IV.  This  hardware-software  implementation  is  tightly  coupled  and  ensures  proper 
communication  within  the  system.  This  communcation  allows  the  users  desired  inputs  to 
be  translated  into  vehicle  motion  in  real-time. 

B.  LESSONS  LEARNED 

The  Shepherd  rotary  vehicle  was  designed  by  Professor  Kanayama  but  built  by 
Mitsubishi  Heavy  Industries  in  Japan.  This  resulted  in  product  not  to  specification,  and  a 
lack  of  legible  documentation.  One  aspect  was  that  the  rotary  vehicle  could  not  fit  into 
the  lab  it  was  planned  for.  Because,  the  size  of  the  robot  was  not  correct.  Also,  the 
documentation  provided  was  in  Japanese,  big  problem.  None  of  the  authors  could  read  or 
speak  Japanese.  This  generated  numerous  faxes  and  telephone  calls  to  Mitsubishi  Heavy 
Industries  requesting  support  and  clarification.  Numerous  hours  were  lost  in  this 
endeavor.  Moreover,  some  of  the  wiring  diagrams  did  not  match  the  actual  wiring 
implementation  on  the  rotary  vehicle. 

The  compiler  used  is  GCC  version  2.7.2. 1.  It  was  good  because  it  was  freeware. 
However,  for  cross-compiler  requirements  of  the  Motorola  68040  processor  it  left  a  lot  to 
be  desired.  Most  notably,  the  lack  of  all  basic  libraries.  All  input-output  functions  and 
math  functions  had  to  be  written  by  the  authors.  Again,  taking  an  inordinate  amount  of 
time  and  effort.  Here  the  authors  were  recreating  the  wheel,  per  se.  Also,  the  unusual 
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handling  of  structure  passing  required  the  development  of  several  work-arounds. 
Essentially,  standard  C-code  writing  had  to  be  modified  extensively.  The  Motorola 
68040  chip  was  designed  to  support  code  written  for  the  Motorola  68020.  This  may 
been  true  but  the  authors  determined  that  for  math  functions  this  was  not  the  case.  This 
also  may  be  related  to  the  compiler  (switches).  The  authors  contacted  Motorola  and 
Omnibyte  Corporations  for  help  in  dealing  with  these  matters  with  little  help  provided. 
In  the  majority  of  cases  the  help  provided  led  to  dead-ends.  Technicians  at  Omnibyte 
recommended  that  a  purchase  of  a  compiler  suitable  for  the  M68040  be  made. 

The  Taurus  Bug  (firmware  on  the  Taurus  Board)  provided  excellent  capabilities. 
However,  a  lot  of  time  was  required  to  use  the  tools.  On  many  occasions  numerous  hours 
were  spent  looking  through  loops  of  assembly  code.  This  would  not  have  been  the  case 
with  a  modem  debugger. 

Group  cross-pollenation  is  essential  for  a  project  with  multiple  disciplines 
involved.  For  example,  a  low  voltage  problem  on  the  rotary  vehicle  caused  the  CPU  to 
slow  or  halt  on  some  occasions.  Many  hours  were  spent  trying  to  determine  what  was 
wrong  with  the  C-coding  and  the  problem  was  hardware.  This  may  have  been  alleviated 
had  there  been  an  electrical  engineer  on  the  team.  Problems  like  this  were  exacerbated 
by  the  documentation  problem  mentioned  earlier.  Another  problem  was  the  overheating 
of  the  boards  on  the  VMEbus  (including  the  CPU)  resulting  in  system  shutdowns  as  well 
as  aberrations  in  CPU  behavior. 


C.  RECOMMENDATIONS  FOR  FUTURE  RESEARCH 

Recommendations  for  future  research  should  include: 

•  Implementation  of  tangential  motion,  the  rudiments  of  which  are  already  in 
place. 

•  Implementation  of  sensing  motion  which  is  required  for  the  percise,  accurate, 
and  safe  detection  of  UXO's. 

•  Transitioning  the  SRK  to  the  PC  environment  possibly  using  LINUX  freeware 
as  the  real-time  operating  kemal. 
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•  Implementation  of  a  M68040  specific  compiler  with  all  libraries. 

•  Addition  of  the  robot  arm  and  implementation  of  the  a  neural  net  and  expert 
system  for  UXO  identification.  Considerations  should  be  given  to  sensors 
such  as  a  magnetometer,  digital  camera  devices,  ground  penetrating  radar, 
xray  devices  and  GPS. 

•  Implementation  of  wireless  ethemet  or  hand  held  computing  devices  for 
control  and  monitoring  of  the  rotary  vehicle  by  a  user  who  is  not  co-located. 

•  Possible  missions  other  than  UXO.  For  instance,  an  unmanned  scout  vehicle 
or  mobile  sentry. 

It  is  obvious  from  the  above  recommendations  that  the  possible  uses  of  the  rotary  vehicle 
is  extensive  and  varied.  The  highly  flexible,  precise,  and  controlled  motion  of  the  vehicle 
makes  it  an  ideal  platform  for  many  ground  applications. 
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APPENDIX  A:  SOURCE  CODE  (MAKEFILE) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 

Edward  Mays,  and  Ferdinand  A.  Reid. 

1  # - * 

2  #  * 

3  # File:  MAKEFILE  * 

4  #  * 

5  #  Environment:  GCC  Compiler  v2.7.2  * 

6  #  Update:  02  February  1997  (Leonardy)  * 

7#  02  April  1997  (Ed  Mays)  * 

8  #  14  May  1997  (Leonardy,  added  utils030.*)  * 

9  #Name:  Thorsten  Leonardy  * 

10#  Purpose:  Makefile  for  project  S  H  E  P  H  E  R  D  .  * 

11  #  * 

12#  Invoke:  make  comp  (to  generate  code)  * 

1 3  #  make  print  (to  print  all  files  to  printer  ap  1  in  Sp-5 11)* 

14#  make  clean  (to  clean  directory  from  object  files)  * 

15#  * 

16# - */ 

17 

18 

19  comp:  startup.o  shepherd.o  timer.o  serial.o  math.o  utils.o  utils030.o  user.o  \ 

20  motor.o  movement.o 

2 1  Id  -Ttext  Ox  1 0000  -Tdata  0x20000  -Tbss  0x30000  -Map  shepherd.map  -oformat 
srec  \ 
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22  -o  shepherd.TXT  startup.o  shepherd.o  timer.o  serial.o  math.o  utils.o  \ 

23  utils030.o  user.o  motor.o  movement.o 

24 

25  shepherd.o :  shepherd.c 

26  gcc  -c  -m68040  -o  shepherd.o  shepherd.c 

27 

28  timer.o :  timer.c 

29  gcc  -c  -m68040  -o  timer.o  timer.c 

30 

31  serial.o :  serial.c 

32  gcc  -c  -m68040  -o  serial.o  serial.c 

33 

34  #servo.o :  servo.c 

35  #  gcc  -c  -m68040  -o  serVo.o  servo.c 

36 

37  utils.o :  utils.c 

38  gcc  -c  -m68040  -o  utils.o  utils.c 

39 

40  utils030.o  :  utils030.c 

41  gcc -c -m68040 -o  utils030.o  utils030.c 

42 

43  user.o :  user.c 

44  gcc  -c  -m68040  -o  user.o  user.c 

45 

46  motor.o  :  motor .c 

47  gcc  -c  -m68040  -o  motor.o  motor.c 
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48 


49  movemento :  movement.c 

50  gcc  -c  -m68040  -o  movement.o  movement.c 

51 

52  startup.o :  startup.s 

53  as  -o  startup.o  startup.s 

54 

55  math.o:  math.c 

56  gcc  -c  -m68040  -o  math.o  math.c 

57 

58 

59  #  This  cleans  out  everything  except  the  Makefile, 

60  #  and  source  files 

61  clean:;  rm  -f  *.o  core 

62 

63  #  This  prints  all  source  files  to  the  printer  apl  in  Sp-51 1 

64  print:;  enscript  -2r  -g  -Papl  makefile  shepherd.map  shepherd.h  shepherd.c  \ 

65  user.h  user.c  utils.h  utils.c  utils030.c  serial.h  serial.c  servo.h  servo.c  \ 

66  timer.h  timer.c  movement.h  movement.c  wheeldrive.h  wheeldrive.c  \ 

67  math.h  math.c  startup.s 

68 

69  #  This  prints  all  source  files  to  the  printer  spl  in  Sp-527 

70  prspl :;  enscript  -2r  -g  -Pspl  makefile  shepherd.map  shepherd.h  shepherd.c  \ 

71  user.h  user.c  utils.h  utils.c  utils030.c  serial.h  serial.c  servo.h  servo.c  \ 

72  timer.h  timer.c  movement.h  movement.c  navigat.h  navigat.c  wheeldrive.h 
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wheeldrive.c  \ 


73  math.h  math.c  motor.h  motor.c  startup.s 

74 

75  $********************************* 

76#  End  of  makefile 

77  ^  ******:i:*************************\ 
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APPENDIX  B:  SOURCE  CODE  (SHEPHERD.C) 


The  following  code  was  modified  by:  Professor  Kanayama*  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /* - * 

2  *  * 

3  *  File:  SHEPHERD  .C  * 

4  *  * 

5  *  Environment:  GCC  Compiler  v2.7.2  * 

6  *  Last  update:  29  January  1997  * 

7  *  Name:  Thorsten  Leonardy  * 

8  *  Purpose:  Provides  the  kernel  for  SHEPHERD.  * 

9  *  * 

10  *  Compiled:  >gcc  -c  -m68040  -o  shepherd.o  shepherd.c  * 

11  *  * 

12  * - */ 

13 

14  #include  "shepherd.h"  /*  general  defines  */ 

15  #include  "movement.h" 

16 
17 

18/* - * 

1 9  *  constant  character  strings  * 


21  unsigned  char  JOYSTICK[26]=  { 
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22 

27,91,49,56,59,54,48,72, 

/*  ESC  [  T  '8' ; 

’6’  'O'  H  */ 

23 

120,61,48,48,48,32, 

/*  'x=000  ’ 

*/ 

24 

121,61,48,48,48,32, 

/*  'y=000 ' 

*/ 

25 

122,61,48,48,48,0}; 

/*  'z=000' 

*/ 

26 

27  unsigned  int  wheelEncoder[8]; 

28  unsigned  char  bcdString[25]= 

29  {24,27,91,48,50,59,52,48,72, 

30  48,48,48,48,48,48,48,48,48,48,48,48,48,48,48,48 } ; 

31 

32  /*  clock  contains  the  current  date/time  updated  every  second  in  ascii  format  */ 

33  /*  it  starts  with  a  count  byte,  the  cursor  positioning  sequence  followed  by  */ 

34  /*  the  date-group  and  the  time  group  in  the  form:  */ 

35  /*  clock=" . mm-dd-yy  hh:mm:ss."  where  7  is  part  of  count  or  ESC  */ 

36  /*  sequence.  */ 

37 

38  /*unsigned  char  clock[27]={  */ 

39/*  25,27,91,48,49,59,54,52,72,  */  /*  ESC  [  ’O’  T  ;  ’6”4'  H  */ 

40  /*  109,109,45,100,100,45,121,121,32,  */  /*  ’mm-dd-yy  '  */ 

41/*  104,104,58,109,109,58,115,115,0};*/ /*'hh:mm:ss’  */ 

42 

43  void  advanceCount() 

44  { 

45  edCounter++; 

46  } 

47 
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48  main() 

49  { 

50  initBoards();  /*  initialize  boards  */ 

51  initMovement();  /*  initialize  movement  */ 

52  sioInit();  /*  initialize  68C681  DU  ART  for  serial  I/O  */ 

53  timerStart();  /*  initialize  and  start  timer-5  fopr  motion  control  */ 

54  /*  setup68030();*//*  setup  OMNI  Module  0  for  serial  I/O  to  VT 100  */ 


55 

enable(); 

/*  enable  all  interrupts,  user  mode 

*/ 

56 

user(); 

/*  let  user  handle  the  main  portion 

*/ 

57 

disableQ; 

/*  disable  interrupts,  supervisor  mode 

*/ 

58 


59  /*  here  goes  downloading  stuff  for  analysis  ...  (i.e.  copy  memory  */ 

60  /*  from  Taurus  Main  memory  to  host  computer  (Laptop  or  SparcStation)  */ 

61 

62  return; 

63 

64  }  /*  end  of  main()  */ 

65 

66 

67 

68  /******************************************************************** 

69  End  of  shepherd.c 

70  ********************************************************************  f 

71 

72 

73 
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APPENDIX  C:  SOURCE  CODE  (USER.C) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /* - * 

2  *  * 

3  *  File:  USER.C  * 

4  *  * 

5  *  Environment:  GCC  Compiler  v2.7.2  * 

6  *  Last  update:  18  February  1997  * 

7  *  Name:  Thorsten  Leonardy  * 

8  *  Purpose:  Provides  the  userpart  for  SHEPHERD.  * 

9  *  * 

10  *  Compiled:  >gcc  -c  -m68040  -o  user.o  user.c  * 

11  *  * 

12  * - */ 

13 

14  #include  "shepherd.h"  /*  general  defines  */ 

15 

16 

17  /* - * 

1 8  *  Global  variables  for  test  program  * 

19  * _ */ 

20 

21  extern  unsigned  char  inPortA;  /*  defined  in  serial.c  */ 

22  extern  unsigned  char  vt!00xy[9];  /*  ESC-Sequence  for  Cursor  Position  */ 
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23  extern  unsigned  char  clrSCR[5];  /*  ESC-Sequence  for  clear  Screen  */ 

24  extern  unsigned  char  clrLine[6];  /*  ESC-Sequence  for  clear  line  */ 

25  extern  unsigned  char  prtSCR[4] ;  /*  ESC-Sequence  for  print  screen  */ 

26  extern  unsigned  char  cursorOFF[5];/*  ESC-Sequence  for  cursor  off  */ 

27 

28  extern  void  gotoXY(int,int); 

29  extern  int  wheelEncoder[8];  /*  defined  in  shepherd.c  */ 

30 

31 

32  extern  char  bcdString[];  /*  defined  in  shepherd.c  */ 

33 

34  unsigned  short  velocity=0; 

35 

36 

37  /* - * 

38  *  constant  character  strings  * 

39  * - */ 

40 

41  #define  MENU.LINES  24 

42  char  *menu[MENU_LINES]={ 

43  "SHEPHERD  Main  Menu  (Last  Update:  27  Feb  97)\n\r", 


44  ••  i  — 5 — o — 5 — 0 — 5 — 0 — 5 — 0 — 5 — 0 — 5 — 0 — 5 — 0 — 5 — 0\n\r", 

45  "\n\r" 

46  "Please  choose:  Diagnostics\n\r", 

47  -  - \n\r". 


48  /*  character  pressed,  analize  ...  */ 
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49  "(1)  Stop  \n\r", 

50  "(2)  Straight  Motion  (Autonomous)  \n\r", 

51  "(3)  Straight  Motion  by  Joystick  \n\r", 

52  "(4)  XY-Motion  by  Joystick  \n\r", 

53  "(5)  Rotate  \n\r", 

54  "(6)  Sinusoidal  Motion  \n\r", 

55  "(7)  Tornado  (External  Center  of  Rotation)\n\r", 

56  "(8)  Tornado  (Internal  Center  of  Rotation)\n\r", 

57  "(9)  Tangential  Motion  \n\r", 

58  "(0)  Exit  Program  \n\r", 

59  "(a)  Tangential  Motion(II)  \n\r", 

60  "(t)  Test  wheel  Angle  4  \n\r", 

61  "  \n\r", 

62  "  \n\r", 

63  "  \n\r", 

64  ”  \n\r", 

65  "  \n\r", 

66  "  \n\r", 

67  " - -\n\r", 

68  " " 

69  }; 

70 

71 

72 

73 

74 
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76  *  displayMenu() 

77  * 

78  *  Environment:  GCC  Compiler  v2.7.2 

79  *  Last  update:  27  January  1997 

80  *  Name:  Thorsten  Leonardy 

81  *  Purpose:  This  function  outputs  a  menu  to  the  screen 

82  * 

83  * - 

84  void  displayMenu(void) 

85  { 

86  int  i; 

87  sioOut(0,clrSCR);  /*  clear  screen  */ 

88  gotoXY(l,l); 

89  for  (i=0;i<MENU JLINES ;  i++) 

90  sioOut(0,menu[i]); 

91  return; 

92  } 

93 

94 

95 

96  /* - 

97  *  status() 

98  * 

99  *  Environment:  GCC  Compiler  v2.7.2 

100  *  Last  update:  18  February  1997 


101*  Name:  Thorsten  Leonardy  * 

102  *  Purpose:  This  function  outputs  a  status  line  at  the  bottom  of  the  * 

103  *  screen.  * 

104  * - */ 


105  void  status(unsigned  char  *p) 

106{ 

107  gotoXY(24,l);  /*  position  cursor  */ 

108  sioOut(0,clrLine);  /*  clear  line  */ 

109  sioOut(0,p);  /*  print  text  */ 

110  return; 

111} 

112 

113 

114/*  - 

115*  convertBCD() 

116* 

117*  Environment:  GCC  Compiler  v2.7.2 
118*  Last  update:  20  February  1997 


119*  Name:  Thorsten  Leonardy  * 

120  *  Purpose:  This  function  converts  an  unsigned  integer  to  a  BCD  string  * 

121  *  of  16  characters.  The  value  is  right  justified  with  leading  * 

122  *  zeros.  * 

123  * - */ 


124 void  convertBCD(unsigned  char  *s,  unsigned  int  data) 

1 25  { 

126  inti=15; 
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127 


128  for  (i=15;  i>=0;  i~)  {  /*  write  16  Bytes,  adjust  integer  to  right  of  string  */ 

129  *(s+i)=48+(data%  10); 

130  data=data/10; 

131  } 

132  return; 

133} 

134 

135/* - * 

1 36  *  converting)  * 

137  *  * 

138  *  Environment:  GCC  Compiler  v2.7.2  * 

139  *  Last  update:  1 1  June  1997  * 


140  *  Name:  Thorsten  Leonardy,  Yutaka  Kanayama  * 

141  *  Purpose:  This  function  converts  a  signed  integer  to  a  BCD  string 

142  *  of  16  characters.  The  value  is  right  justified  with  leading  * 

143  *  zeros.  * 

144  * - */ 


145 void  convertInt(unsigned  char  *s,  int  data) 

146{ 

147  int  i=15; 

148 

149  if(data>=0) 

150  *s  =  32;  /*  space  for  positive  number  */ 

151  else 

152  { 


86 


153 


*s  =  45;  /*  minus  sign  for  negative  number  */ 
data  =  -data; 


154 

155  } 

156 

157  for  (i=15;  i>=l;  i~)  {  /*  write  16  Bytes,  adjust  integer  to  right  of  string  */ 

158  *(s+i)=48+(data%  10); 

159  data=data/10; 

160  } 

161  return; 

162} 

163 

164 

165 

166/* - * 

167  *  wheelDrive()  * 

168  *  * 

169  *  Environment:  GCC  Compiler  v2.7.2  * 

170  *  Last  update:  27  February  1997  * 

171*  Name:  Thorsten  Leonardy  * 

172  *  Purpose:  This  function  drives  the  specified  wheel  with  required  * 

173  *  velocity.  * 

174  *  * 

175  * - */ 

176void  wheelDrivel  (unsigned  short  wheel,  unsigned  short  velo) 

1 77  { 

178  unsigned  short  *servoOut=(unsigned  short  *)0xffff0482;  /*  analog  out  */ 
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179  unsigned  int  *servoControl=(unsigned  int*)OxffffffOO;  /*  Data  out  */ 

180 

181  *servoOut=velo;  /*  set  velocity  first  */ 

182 

183  if  (wheel) 

184  *servoControl=0x00000004; 

185  else 

1 86  *  servoControl=0x00000000 ; 

187 

1 88  return; 

189} 

190 

191 

192 

193 

194 

196  *  updateWheelStatus()  * 

197  *  * 

198  *  Environment:  GCC  Compiler  v2.7.2  * 

199  *  Last  update:  27  February  1997  * 

200  *  Name:  Thorsten  Leonardy  * 

201  *  Purpose:  This  function  reads  the  current  shaft  encoder  readings  for  * 

202  *  all  eight  servo  motors  and  outputs  them  to  the  screen.  * 

203  *  * 

204  *  unsigned  int  wheelEncoder[8]  -  array  to  hold  the  shaft  encoder  readings  * 
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205  *  unsigned  char  *bcdString  -  string  to  hold  converted  encoder  reading  * 
206*  * 

207  * - */ 

208void  updateWheelStatus(void) 

209  { 

210 

2 1 1  unsigned  short  i,posx,posy; 

212 

213  readWheelStatus(wheelEncoder);  /*  read  wheel  status:  File  servo.c  */ 

214 

215  posx=8;  /*  x-position  on  screen  for  reading  motor  1  */ 

216  posy=40;  /*  y-position  on  screen  for  reading  motor  1  */ 

217 

218  for  (i=0;  i<8;  i++)  { 

219  posx=8+i%4;  /*  position  for  x  */ 

220  posy=40+20*(i/4);  /*  position  for  y  */ 

221  bcdString[3]=48+posx/10;  /*  convert  tens  to  ascii  */ 

222  bcdString[4]=48+posx%10;  /*  convert  ones  to  ascii  */ 

223  bcdString[6]=48+posy/10;  /*  convert  tens  to  ascii  */ 

224  bcdString[7]=48+posy%10;  /*  convert  ones  to  ascii  */ 

225  convertBCD(bcdString+9,wheelEncoder[i]);  /*  convert  reading  itself  */ 

226  WRITE_ENCODER();  /*  output  ascii  */ 

227  }  /*  end  of  for*/ 

228 

229  return; 

230} 
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231 


232 

233void  displayDriveSpeed() 

234  { 

235  double  speed00,speed0,speedl,speed2,speed3; 

236 

237  disable(); 

238  speed00=desiredSpeeds[0]; 

239  speedO=Display_Speeds[0]; 

240  speedl=Display_Speeds[l]; 

241  speed2=Display_Speeds[2]; 

242  speed3=Display_Speeds[3]; 

243  Display_Speeds[0]=0.0; 

244  Display_Speeds[l]=0.0; 

245  Display_Speeds[2]=0.0; 

246  Display_Speeds[3]=0.0; 

247 

248  enable(); 

249 

250  con  vertInt(bcdS  tring+9 ,  speedOO) ; 

251  bcdString[3]='0'; 

252  bcdString[4]='3'; 

253  bcdString[6]=’4’; 

254  bcdString[7]='0'; 

255  sioOut(0,bcdString); 

256  convert!nt(bcdString+9,speedO); 
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257  bcdS  tring  [  3  ] ='0' ; 

258  bcdString[4]=’3'; 

259  bcdString[6]='6'; 

260  bcdStrmg[7]='0'; 

261  sioOut(0, bedString); 

262  convertInt(bcdS  tring+9,  speed  1 ); 

263  bcdString[3]='0'; 

264  bcdString[4]='4'; 

265  bcdString[6]='6'; 

266  bcdString[7]='0'; 

267  sioOut(0, bedString); 

268  convertInt(bcdString+9,speed2); 

269  bcdString[3]=’0'; 

270  bcdString[4]='5'; 

271  bcdString[6]=’6'; 

272  bcdString[7]='0'; 

273  sioOut(0, bcdS  tring); 

274  convertInt(bcdString+9,speed3); 

275  bcdString[3]=’0'; 

276  bcdString[4]=,6'; 

277  bcdString[6]='6'; 

278  bcdString[7]=’0'; 

279  sioOut(0,bcdString); 

280  return; 

281} 

282 
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283void  displayDriveSteer() 

284  { 

285  double  steerOO,steerO,steerl,steer2,steer3; 

286 

287  disable(); 

288  steerOO=Steer_Digits[0]; 

289  /*  steerOO=desiredAngleRates[0];  */ 

290  steerO=desiredAngleRates  [0]  *  1 000; 

291  steerO=Display_Steers[0]; 

292  steerl=Display_Steers[l]; 

293  steer2=Display_Steers[2]; 

294  steer3=Display_Steers[3]; 

295  Display_Steers[0]=0.0; 

296  Display_Steers[l]=0.0; 

297  Display_Steers[2]=0.0; 

298  Display_Steers[3]=0.0; 

299 

300  enable(); 

301 

302  convertInt(bcdString+9,steer00); 

303  bcdString[3]='0’; 

304  bcdString[4]='3’; 

305  bcdString[6]='4'; 

306  bcdString[7]='0'; 

307  sioOut(0,bcdString); 

308  convert!nt(bcdString+9,steerO); 


309  bcdS  tring  [3  ]='0’ ; 

310  bcdString[4]='3'; 

3 1 1  bcdString[6]='6'; 

312  bcdString[7]='0'; 

3 1 3  sioOut(0, bedString); 

3 14  convertInt(bcdString+9,steerl); 

315  bcdString[3]='0'; 

316  bcdString[4]=’4’; 

317  bcdString[6]='6'; 

318  bcdString[7]='0'; 

319  sioOut(0,bcdString); 

320  convertInt(bcdString+9,steer2); 

321  bcdString[3]='0'; 

322  bcdString[4]='5’; 

323  bcdString[6]=’6'; 

324  bcdString[7]='0'; 

325  sioOut(0, bedString); 

326  convertInt(bcdString+9,steer3); 

327  bcdString[3]='0'; 

328  bcdString[4]='6'; 

329  bcdString[6]='6'; 

330  bcdString[7]='0'; 

331  sioOut(0,bcdString); 

332  return; 

333} 

334 
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335void  display  Angles() 

336  { 

337  double  steer0,steerl,steer2,steer3; 

338 

339  disable(); 

340 

34 1  steerO=actual  AnglesfO]  *  1 000*RadsToDegrees ; 

342  steerl=actualAngles[l]*  1000*RadsToDegrees; 

343  steer2=actualAngles[2]*  1000*RadsToDegrees; 

344  steer3=actual Angles [3]*  1000*RadsToDegrees; 

345 

346  enable(); 

347 

348  con  vertInt(bcdS  tring+9  ,steerO) ; 

349  bcdString[3]='0’; 

350  bcdString[4]='3'; 

351  bcdString[6]='6'; 

352  bcdString[7]='0'; 

353  sioOut(0,bcdString); 

354  convertInt(bcdString+9, steer  1 ); 

355  bcdString[3]='0'; 

356  bcdString[4]='4'; 

357  bcdString[6]='6'; 

358  bcdString[7]='0'; 

359  sioOut(0,bcdString); 

360  convert!nt(bcdString+9,steer2); 
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361  bcdString[3]='0'; 

362  bcdS  tring  [4] ='5 ' ; 

363  bcdString[6]='6'; 

364  bcdString[7]='0'; 

365  sioOut(0, bcdString); 

366  convertInt(bcdString+9,steer3); 

367  bcdString[3]='0'; 

368  bcdString[4]='6'; 

369  bcdString[6]='6'; 

370  bcdString[7]='0'; 

371  sioOut(0, bcdString); 

372  return; 

373} 

374void  displayVehicleConfigO 

375  { 

376  double  coordx,  coordy,  heading,  kappa; 

377 

378  disable(); 

379 

380  coordx  =  vehicle.coord.x; 

38 1  coordy  =  vehicle.coord.y; 

382  heading  =  vehicle. heading; 

383  kappa  =  vehicle.kappa; 

384 

385  enable(); 

386 
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387  convertInt(bcdString+9,coordx); 

388  bcdString[3]='0'; 

389  bcdString[4]='3'; 

390  bcdString[6]='6'; 

391  bcdString[7]='0'; 

392  sioOut(0,bcdString); 

393  convertInt(bcdString+9,coordy); 

394  bcdString[3]='0'; 

395  bcdString[4]='4'; 

396  bcdString[6]='6'; 

397  bcdString[7]='0’; 

398  sioOut(0,bcdString); 

399  convertInt(bcdString+9, heading); 

400  bcdString[3]='0'; 

401  bcdString[4]='5'; 

402  bcdString[6]='6'; 

403  bcdString[7]=’0'; 

404  sioOut(0,bcdString); 

405  convertInt(bcdString+9, kappa) ; 

406  bcdString[3]='0'; 

407  bcdString[4]='6'; 

408  bcdString[6]='6’; 

409  bcdString[7]='0’; 

410  sioOut(O.bcdString); 

4 1 1  return; 

412} 
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413/* 


* 


-r  u/  - 

414  *  user() 

* 

415* 

* 

416  *  Environment:  GCC  Compiler  v2.7. 2 

* 

417  *  Last  update:  18  Febmary  1997 

* 

418  *  Name:  Thorsten  Leonardy 

* 

419  *  Purpose:  This  function  provides  the  user  shell. 

* 

420* 

* 

421  * - 

- */ 

422 

423void  user(void) 

424  { 

425  inta; 

426  char  *s; 

427  unsigned  int  *servoControl=(unsigned  int  *)VME2170;/*  test  only  */ 

428  displayMenu();  /*  display  menu  */ 

429  do 

430  { 

43 1  inPortA='?';  /*  reset  character  */ 

432  while(inPortA='?');  /*  wait  for  character  to  be  typed  in  */ 

433  /*  character  pressed,  analize  ...  */ 

434  switch(inPortA) 

435  { 

436  case  T  :  if  (mode  !=  5)  /*  Stop  */ 

437  { 

438  modeOstate  =  0; 
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439 

440 

441 

442 

443 

444 

445 

446 

447 

448 

449 

450 

451 

452 

453 

454 

455 

456 

457 

458 

459 

460 

461 

462 

463 

464 


mode  =  1; 

while  (modeOstate  ==0)  { }; 

disable(); 

align(); 

enable(); 


/*  *servoControl=0x00429429;  test  by  Ed  */ 
modeOstate  =  2; 

} 

mode  =  1 ; 
disable(); 

alignAfterRotate(); 

enable(); 

/*  *servoControl=0x00429429;  test  by  Ed*/ 


} 

initMovement(); 

break; 

case  '2' :  mode  =  2;  /*  Straight  Motion  (Autonomous)  */ 
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465  break; 

466 

467  case  '3' :  mode  =  3;  /*  Straight  Motion  by  Joystick  */ 

468  break; 

469 

470  case  '4' :  mode  =  4;  /*  X-Y  Motion  by  Joystick  */ 

471  /*  for  (a=0;a<100;a++){  */ 

472  /*  while  ((edCounter  %  200  !=  0)  &&  (a  !=  100)){*/ 

473  /*  display  Angles();  */ 

474  /*  }  */ 

475  /*  }  */ 

476  break; 

477 

478  case  '5* :  mode5state  =  0;  /*  Rotate  */ 

479  mode  =  5; 

480  break; 

481 

482  case  '6' :  mode  =  6;  /*  Sinusoidal  Motion  */ 

483  break; 

484 

485  case  7' :  mode  =  7;  /*  Tornado  (Center  of  Rot  External)  */ 

486  break; 

487 

488  case  ’8' :  mode  =  8;  /*  Tornado  (Center  of  Rot  Internal)  */ 

489  break; 

490 
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491 

case  '9' 

:  mode  =  9;  /*  Tangential  Motion  */ 

492 

initTangent(); 

493 

while(l){ 

494 

while(edCounter%200  !=  0){ }; 

495 

display  VehicleConfigO ; 

496 

}; 

497 

break; 

498 

499 

case 

'a' :  mode  =  10;  /*  Tangential  Motion  (13)  */ 

500 

break; 

501 

502 

case 

't' :  modeTstate  =  0;  /*  Steering  test  mode  */ 

503 

/*  Flag  =  1 ;  initialized  in  movement.c  */ 

504 

mode  =  100; 

505 

while  (1) 

506 

{ 

507 

oldFlag  =  Flag; 

508 

while  (Flag  ==  oldFlag)  { } 

509 

display  An  gles(); 

510 

} 

511 

break; 

512 

513 

default 

:  break; 

514  }  /*  end  of  switch  */ 

515  }  while(inPortA!='0');/*  end  of  while,  exit  with  'O'  entered  at  keyboard  */ 


517  sioOut(0,clrSCR);  /*  clear  screen  */ 

518  sioOut(0,"\n\r\n\r");  /*  some  cr,lf  */ 

519 

520  return; 

521 

522  while(l) 

523  { 

524  while(edCounter%200  !=  0)  { } ; 

525  /*  displayJoyStick();  */ 

526  displayDriveSteer(); 

527  /*  display Angles();  */ 

528  }; 

529  sioOut(0,cursorOFF);  /*  switch  cursor  off  (no  blink)  */ 

530 

531 }  /*  end  of  user()  */ 

532 

533 

534 

535asm(" 

536  .even 

537  .text 

538  .globl  _WRITE_ENCODER 

539 

540_WRITE_ENCODER : 

541 

542  pea.l  _bcdString 


101 


543 


544  trap  #15 

545  dc.w  0x0023 

546 

547  rts 
548"); 

549 

550 

551 

552/**************************************************************** 

553  Endofuser.c 

554.  ******************************************:*:**:*::*:******:*::*:***:*::*:***:*:/ 

555 
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APPENDIX  D:  SOURCE  CODE  (MOVEMENTS) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  #include  "shepherd.h" 

2  #include  "movement.h" 

3  #include  "math.h" 

4 

5  /* - * 

6  *  Main  * 

7  * - */ 

8  void  driver() 

9  { 

10  readEncoders();  /*  Read  Drive/Steer  Motors  */ 

1 1  compute  ActualRates(); 

12  /*accumulateDriveSpeed();  only  for  wheel  speed  displaying  */ 

13  accumulateDriveSteer(); 

14  bodyMotion(); 

15  wheelMotion(); 

16  /*  testDrivel();  */ 

17  driveMotors(); 

1 8  advanceCount(); 

19  } 


103 


20 


21 

22  /* - * 

23  *  Initialize  Movement:  * 

24  *  intialize  Configuration  and  vehicle  motion  * 

25  * - */ 

26  void  initMovement() 

27  { 

28  int ix; 

29 

30  Flag  =  1 ; 

31  oldMode  =  0; 

32  mode  =  1 ; 

33  Omega_Speed=0.0; 

34  testCounter=0; 

35  edCounter=0; 

36  pathLength=0.0; 

37 

38  K1  [0]=1 1 .448 ;/*0<=speed<=5 ,  */ 

39  Kl[l]=l  1.500; 

40  Kl[2]=  11.496; 

41  Kl[3]=  12.375; 
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42 

43  K2[0]=l  1 .500;/*5>speed<8*/ 

44  K2[l]=l  1.500; 

45  K2[2]=l  1.644; 

46  K2[3]=12.000; 

47 

48 

49  K3[0]=l  1 .61 1  ;/*8>=speed<20*/ 

50  K3[l]=l  1.585; 

51  K3[2]=l  1.686; 

52  K3[3]=l  1.840; 

53 

54  K4[0]=l  1 .7 1 1  ;/*20>=speed<=70*/ 

55  K4[l]=l  1.659; 

56  K4[2]=l  1.705; 

57  K4[3]=l  1.727; 

58 

59  K6[0]=l  1 .7 10;/*70>speed<K5*/ 

60  K6[l]=l  1.700; 

61  K6[2]=  11.700; 

62  K6[3]=  11.715; 

63 
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64 


65  DigitToCmDrive[0]=  +0.001 1369287;/*  driving  constant  cm/count 
digitToRadDrive*  1 8.9cm*/ 

66  DigitToCmDrive[l]=  -0.0011369287; 

67  DigitToCmDrive[2]=  +0.001 1369287; 

68  DigitToCmDrive[3]=  -0.001 1369287; 

69 

70 

71  motion.Speed=0.0; 

72  motion.Theta=0.0; 

73  motion.Omega=0.0; 

74 

75  radius  =100; 

76 

77  vehicle.coord.x=0.0; 

78  vehicle.coord.y=0.0; 

79  vehicle.heading=0.0; 

80  vehicle.kappa=l /radius; 

81 

82  ai[0]  =  40.0;  ai[  1  ]  =  40.0;  ai[2]  =-40.0;  ai[3]  =-40.0; 

83  bi[0]  =-40.0;  bi[  1  ]  =  40.0;  bi[2]  =-40.0;  bi[3]  =  40.0; 

84 
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85 


86  joyStick.x  =  0.0; 

87  joyStick.y  =  0.0; 

88 

89  setupPolar(whp); 

90 

91  for  (ix  =0;  ix  <ARRAY_SIZE;  ix++){ 

92  PreviousCountSpeed[ix]=99999999; 

93  PreviousCountSteer[ix]=99999999; 

94  Display_Speeds[ix]=0.0; 

95  Display_Steers[ix]=0.0; 

96  actualAngles[ix]=0.0; 

97  desiredSpeeds[ix]  =  0.0; 

98  desired  AngleRates[ix]  =  0.0; 

99  desiredAngleRatesO[ix]  =  0.0; 

100  desiredAngles[ix]=0.0; 

101  desiredAngles0[ix]=0.0; 

102  WheelDirActO[ix]=  1.0e8; 

103  WheelDirAct[ix]  =  0.0; 

104  WheelDirDes[ix]  =  0.0; 

105  steerReadings[ix]=0.0;  /*  not  used  only  testing  */ 

106  driveReadings[ix]=0; 


107 


107  } 


108} 

109 

110 

111/* - * 

112  *  SetupPolar  * 

113  * - */ 

114void  setupPolar(polar  whp[4]) 

1 15  { 

1 16  whp[0].rho  =  whp[l].rho  =  whp[2].rho  =  whp[3].rho  =  56.5685425; 

117  /*  distances  =  40  *  sqrt(2)  */ 

118  whp[0]. alpha  =  -QPI;  /*  front  right  wheel  1  */ 

119  whp[  1]. alpha  =  QPI;  /*  front  left  wheel  2  */ 

120  whp[2].alpha  = -3.0*QPI;  /*  rear  right  wheel  3  */ 

121  whp[3]. alpha  =  3.0*QPI;  /*  rear  left  wheel  4  */ 

122} 

123 

124 

125/* - * 

1 26  *  bodyMotion  —  Updates  Vehicle  * 

127  * - */ 

128  void  bodyMotion() 
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129  { 

130  double  vO,  omegaO, 

131  linSpeed=  4.0, 

132  linAcc  =  1.0, 

133  rotSpeed=  0.1,  /*  0.05,  */ 

134  rotAcc  =0.025,  /*  0.0125;  */ 

135  RPI  =  QPP1.5;  /*  67.5  degrees  */ 

136  double  theta,  omega,  speed; 

137 

138  speed  =  motionO.Speed  =  motion.Speed;  /*  save  the  previous  motion  */ 

139  theta  =  motionO.Theta  =  motion.Theta;  /*  for  computing  derivatives  */ 

140  omega  =  motionO.Omega  =  motion. Omega; 

141 

142  switch(mode){ 

143  case  1: 

144  if  (modeOstate  ==  2) 

145  break; 

146  if  ( (Speed_Digits[0]  =  0)  &&  (Speed_Digits[  1  ]  =  0)  && 

147  (Speed_Digits[2]  ==  0)  &&  (Speed_Digits[3]  =  0)  && 

148  (Steer_Digits[0]  =  0)  &&  (Steer_Digits[  1  ]  =  0)  && 

149  (Steer_Digits[2]  =  0)  &&  (Steer_Digits[3]  =  0)) 

1 50  modeOstate  =  1 ; 
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151 


/*  allStopQ;  will  be  inserted  later  */ 


152  break; 

153 

154  case  2: 

155  speed  =  min(speed  +  2.0*DeltaT,  10.0); 

156  break; 

157 

158  case  3:  /*  Straight  Motion  by  Joystick  */ 

159  readJoyStick();  /*  ejm  19  july  97  */ 

1 60  speed  =  -joyStick.y*0. 1 ; 

161  theta  =  0.0; 

162  omega  =  0.0; 

163  break; 

164 

165  case  4:  /*  X-Y  Motion  by  Joystick  */ 

166  readJoyStick();  /*  ejm  19  july  97*/ 

167  speed  =  -joyStick.y*0.1;  /*  speed  control  */ 

168  theta  =  -joyStick.x*0.02;  /*  steering  control  */ 

169  if  (theta  >  HPI)  theta  =  HPI; 

1 70  if  (theta  <  -HPI)  theta  =  -HPI; 

171  /*  omega  =  -joyStick.omega*0. 1  ;*/  /*  24  july  97  */ 

172  break; 
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173 


174  case  5: 

175  if  (mode5  state  ==  1 )  { 

176  readJoyStick(); 

177  speed  =  -joyStick.y*0. 1; 

178  } 

179  break; 

180 

181  case  6:  /*  sinusoidal  motion  */ 

182  speed  =  min(speed  +  HnAcc*DeltaT,  10.0); 

1 83  speed  =  speed; 

184  if  (speed  ==  10.0){ 

185  pathLength  +=  DeltaT*speed; 

1 86  theta  =  0.4  *  sin(pathLength/20.0);  /*  sine  curve  motion  */ 

187  } 

188  break; 

189 

190  case  7:  /*  Tornado  External  */ 

1 9 1  speed  =  min(speed  +  1 .0*DeltaT,  8.0); 

192  if  (  speed  =  8.0) 

193  omega  =  min(omega  +  0.01 25*DeltaT,  0.1);  /*  radius  =  80  cm  */ 

194  break; 
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195 


196  case  8:  /*  Tornado  Internal  */ 

197  speed  =  min(speed  +  1.0*DeltaT,  8.0); 

198  if  ( speed  ==  8.0) 

1 99  omega  =  min(omega  +  0.025*DeltaT,  0.2);  /*  radius  =  40  cm  */ 

200  break; 

201 

202  case  9:  /*  tangential  motion  */ 

203  tangentialMotion(); 

204  break; 

205 

206  case  10:  /*  tangential  motion  (II)  */ 

207  speed  =  min(speed  +  linAcc*DeltaT,  8.0); 

208  break; 

209 

210  case  100: 

211  break; 

212  } 

213 

214  if(mode!=9){ 

215  motion. Speed  =  speed; 

216  motion  .Theta  =  theta; 
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217  motion.Omega  =  omega; 

218 

2 1 9  vehicle.heading  =  vehicle.heading  +  motion.Omega*DeltaT ; 

220  vehicle.coord.x  =  vehicle.coord.x  +  motion. Speed*DeltaT  *  cos(motion.Theta); 

221  vehicle.coord.y  =  vehicle.coord.y  +  motion.Speed*DeltaT  *  sin(motion. Theta); 

222 

223  speedDot=(motion.Speed  -  motionO.Speed)/DeltaT; 

224  thetaDot=(motion  .Theta  -  motionO.Theta)/DeltaT ; 

225  omegaDot=(motion .  Omega  -  motionO.  Omega)/DeltaT ; 

226  } 

227} 

228 

229 

230/* - * 

23 1  *  wheelMotion  * 

232  * - */ 

233  void  wheelMotion() 

234  {  /*the  function  that  truly  belongs  here  is  in  calculate.org  */ 

235  int  i; 

236  double  v lx,  vly,  vlyvlxRatio; 

237  double  theta=motion.Theta, 

238  omega=motion.Omega, 
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239 


speed=motion.Speed, 


240  Omega2=omega*omega, 

241  Omega3=Omega2*omega, 

242  beta,ro,ro2, 

243  wheel  AngleV; 

244 

245  if(mode==5){  /*  rotate  case  */ 

246  s  witch(mode5  state)  { 

247  case  0: 

248  /*  turn  each  wheel  by  +-PI/4  in  5  seconds  */ 

249  desired  An  gles[0]  +=  QPIby500;  /*  =  (PI/4)/500  */ 

250  desiredAngles  [  1  ]  -=  QPIby500; 

251  desiredAngles  [2] -=  QPIby500; 

252  desiredAngles[3]  +=  QPIby500; 

253  if  (desiredAngles  [0]  >=  QPI) 

254  mode5  state  =  1 ; 

255  break; 

256 

257  case  1 :  /*  drive  wheels  to  rotate  body  */ 

258  desiredSpeeds[0]  =  +speed; 

259  desiredSpeeds[  1  ]  =  -speed; 

260  desiredSpeeds[2]  =  +speed; 
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desiredSpeeds[3]  =  -speed; 
break; 


261 
262 

263  } 

264  return; 

265  } 

266 

267  for(i=0;i<4;i++){  /*  non-rotate  case  */ 

268  ro=whp[i].rho; 

269  ro2=ro*ro; 

270  beta=vehicle.heading+whp[i].alpha; 

271  vlx  =  speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 

272  vly  =  speed*sin(theta)+(whp[i].rho*omega*cos(beta)); 

273  desiredSpeeds[i]  =  new_sqrt(vlx*vlx  +  vly*vly); 

274 

275  switch(mode){ 

276  case  1: 

277  case  2: 

278  case  3: 

279  if  (speed  <  0.0) 

280  desiredSpeeds[i]  =  -desiredSpeeds[iJ; 

281  if  (new_abs(vlx)  >  0.01){ 

282  vlyvlxRatio=vly/vlx; 
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283 

284 

285 

286 

287 

288 

289 

290 

291 

292 

293 

294 

295 

296 

297 

298 

299 

300 

301 

302 

303 


desiredAngles[i]  =  atan(vlyvlxRatio)  -  vehicle.heading; 
wheel  Angle  V  =  motion.Theta  -  vehicle.heading  -  whp[i]. alpha; 
desiredAngleRates[i]  = 

( (speed*speed*thetaDot  +  ro2*Omega3) 
+speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 
/( desiredSpeeds[i]*  desiredSpeeds[i])  -  omega; 
desiredAnglesO[i]  =  desiredAngles[i]; 
desiredAngleRatesO[i]  =  desiredAngleRates[i]; 

} 

else{ 

desired  Angles  [i]  =  desiredAnglesO[i]; 
desiredAngleRates[i]  =  desiredAngleRatesO[i]; 

} 

break; 

case  4: 

if  (speed  <  0.0) 

desiredSpeedsp]  =  -desiredSpeeds[i]; 
if  (new_abs(v  1  x)  >  0.0 1 )  { 


304 


v  1  y  v  1  xRatio= v  1  y/v  1  x ; 
desiredAngles[i]  =  theta; 
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305  desired  AngleRates[i]  =  0.0; 

306  desired  Angles0[i]  =  desiredAngles[i]; 

307  desiredAngleRatesO[i]  =  desiredAngleRates[i]; 

308  } 

309  else{ 

310  desiredAngles[i]  =  desiredAnglesO[i]; 

311  desired  AngleRates[i]  =  desiredAngleRatesO[i] ; 

312  } 

313  break; 

314 

315  case  6: 

316  case  7: 

317  case  8: 

318  case  9: 

319  if  (new_abs(desiredSpeeds[i])  >  0.01){ 

320  desiredAngles[i]  =  atan2(vly,vlx)  -  vehicle.heading; 

321  wheelAngleV  =  motion.Theta  -  vehicle.heading  -  whp[i]. alpha; 

322  desiredAngleRates[i]  = 

323  ( (speed*speed*thetaDot  +  ro2*Omega3) 

324  +speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 

325  +ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 

326  /(  desiredSpeeds[i]*  desiredSpeeds[i])  -  omega; 


117 


327 


desiredAnglesO[i]  =  desiredAngles[i]; 
desiredAngleRatesO[i]  =  desiredAngleRates[i]; 


328 

329  } 

330  else{ 

331  desired  Angles  [i]  =  desired  Angles0[i]; 

332  desired  AngleRates[i]  =  desiredAngleRatesO[i]; 

333  } 

334  break; 

335 

336  case  10: 

337  desiredSpeeds[i]  =  speed  * 
(new_sqrt((ai[i]*vehicle.kappa)*(ai[i]*vehicle.kappa) 

338  +(l-bi[i]*  vehicle. kappa)*(l-bi[i]*vehicle. kappa))); 

339  if  (vehicle.kappa  !=  0.0)  { 

340  desired  Angles  [i]  =  atan2(bi[i],(vehicle.kappa-ai[i])); 

341  } 

342  else  {  desiredAngles[i]  =  0.0;} 

343  desiredAngleRates[i]  =  0.0; 

344  break; 

345  case  100: 

346  break; 

347  }/*  end  switch  */ 
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348  }/*  end  for  */ 

349} 

350 

351 

352 

353/* - * 

354  *  joystickMotionlnterface  ejm  19  June  97  *  * 

355  * - */ 

356void  joystickMotionInterface() 

357  { 

358  motion.Speed  =  joyStick.y;  /*  convert  x-position  into  double  */ 

359  motion.Theta  =  joyStick.x;  /*  convert  y-poistion  into  double  */ 

360  motion.Omega  =0.0;  /*motion.Omega  =  joystick. w;  not  implemented  yet;*/ 

361} 

362 

363/* - * 

364  *  tangentialMotion  * 

365  * - */ 

366void  tangentialMotion() 

367  { 

368  double  deltaTheta,  deltax,  deltay,  Si,  totalDistance,  deltaDistance; 

369  int  ix; 
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370 


371  deltax  =  0; 

372  deltay  =  0; 

373  for  (ix  =  0;  ix  <  4;  ix++){ 

374  deltax  =  deltax  +  actualSpeeds[ix]*cos(actualAngles[ix]); 

375  deltay  =  deltay  +  actualSpeeds[ix]*sin(actualAngles[ix]); 

376  } 

377 

378  /*retums  the  linear  distance  the  vehicle  has  travelled  */ 

379  deltaS  =  (DeltaT/4)*new_sqrt((deltax*deltax)+(deltay*deltay)); 

380 

381  /*  returns  the  difference  between  the  changes  in  the  distance  */ 

382  /*  of  the  left  and  right  wheels  */ 

383  deltaTheta  =  0.0; 

384  for  (ix  =  0;  ix  <  4;  ix++){ 

385  Si  =  actualSpeeds[ix]*DeltaT; 

386  deltaTheta  =  deltaTheta  +  (sin(actualAngles[ix])/ai[ix] 

387  -  cos(actualAngles[ix])/bi[ix])*Si; 

388  } 

389  deltaTheta  =  deltaTheta/4; 

390 

391  totalDistance  +=  deltaS;  /*  Keeps  track  of  the  total  distance  traved  by  vehicle  */ 
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392 

393  /*  update  the  vehicle's  configuration  based  on  the  distance  travelled  */ 

394  /*  during  the  last  motion  control  cycle  */ 

395 

396  vehicle.heading  +=  deltaTheta; 

397  circularArc(deltaS,  deltaTheta); 

398  compose(); 

399 

400  deltaDistance  =  DeltaT*motion.  Speed; 

40 1  /*  vehicle.kappa  +=  (steer()*deltaDistance);*/ 

402  vehicle.kappa  =  0.0; 

403/*  motion.Theta  +=  deltaTheta;  */ 

404/*  motion.Theta  =  vehicle.heading;  */ 

405  motion.Theta  =  0.0; 

406/*  motion. Omega  =  vehicle.kappa*motion. Speed;  */ 

407  motion. Omega  =  0.0; 

408  thetaDot  =  deltaTheta/DeltaT; 

409  speedDot  =  0.0; 

4 1 0  omegaDot  =  0.0; 

411} 

412 

413/*******************************************************************/ 
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414/*  FUNCTION:  circularArc()  */ 

415/*  PARAMETERS:  Configuration  length  -the  arc  length  */ 

416/*  alpha  —the  end  orientation  */ 

417/*  config  —pointer  to  the  resultant  configuration  */ 

418/*  PURPOSE:  Given  the  arc  length  and  alpha,  to  calculate  the  final  */ 

419/*  configuration  */ 

420/*  RETURNS:  Configuration:  pointer  to  the  final  configuration  */ 

421/*  COMMENTS:  The  main  purpose  of  this  function  is  to  be  used  in  conjunction  */ 
422/*  with  compose()  to  form  a  new  next().  In  this  case,  length  would*/ 


423/*  actually  be  delta-s  and  alpha  would  be  delta-theta.  */ 

424/*  Circular_arc()  would  determine  the  configuration  after  the  incre-*/ 

425/*  mental  move  in  the  local  coordinate  system  of  the  original  */ 

426/*  configuration.  Then  compose()  would  take  the  original  */ 

427/*  configuration  (in  global  coordinates)  and  the  incremental  */ 


428/*  configuration  (in  local  coordinates)  to  determine  the  */ 

429/*  incremental  configuration  in  global  coordinates.  */ 

431  void 

432circularArc(double  length,  double  alpha) 

433  { 

434 

435  double  alpha2,alpha4; 
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436 


437  alpha2=  alpha*alpha; 

438  alpha4=  alpha2*alpha2; 

439  defineConfig((l-  alpha2/6.0  +  alpha4/120.0)  *  length, 

440  (0.5  -  alpha2/24  +  alpha4/720.0)  *  alpha  *  length, 

441  alpha,  0.0); 

442} 


444/*  FUNCTION:  defineConfigO  */ 

445/*  PARAMETERS:  double  x,y, theta, kappa  —The  values  that  define  a  */ 

446/*  configuration  */ 

447/*  PURPOSE:  To  allocate  nad  assign  a  configuration  */ 

448/*  RETURNS:  Configuration:  a  configuration  */ 

449/*  COMMENTS:  Was  called  def_configuration()  in  MML10  */ 


450/*****************************************************************/ 

45 1  void 

452defineConfig(double  x,double  y,double  theta, double  kappa) 

453  { 

454  incrementalMotion.coord.x  =  x; 


455  incrementalMotion.coord.y  =  y; 

456  incrementalMotion.heading  =  theta; 

457  incrementalMotion. kappa  =  kappa; 
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458} 


459 

460 

451/********************************************************************/ 

462/*  FUNCTION:  compose()  */ 

463/*  PARAMETERS:  Configuration  *first  -  pointer  to  the  first  configuration  */ 
464/*  *  second  --  pointer  to  the  second  configuration  */ 


465/*  PURPOSE:  To  calculate  the  composition  of  the  first  and  second  */ 

466/*  configurations  */ 

467/*  RETURNS:  Configuration:  configuration  which  is  the  */ 

468/*  composition  of  the  first  and  second  configurations  */ 


469/*  COMMENTS:  A  typical  example  of  the  usage  of  this  function  is  to  determine  */ 
470/*  the  goal  position  of  a  configuration  in  global  coordinates.  In  */ 


471/*  such  an  example,  the  first  argument  would  be  the  original  */ 

472/*  configuration  and  the  second  argument  would  be  the  goal  */ 

473/*  configuration  in  the  original  configuration's  local  coordinate  */ 

474/*  system.  The  resultant  third  argument  would  then  be  the  goal  */ 


475/*  configuration  in  global  coordinates.W as  called  comp()  in  MML10  */ 

476/*  LAST  UPDATE:  10/25/94  Chien-Liang  Chuang  */ 

477/*******************************************************************/ 

478void 

479compose() 
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480  { 

481 

482  double  x,y,  theta; 

483  double  xx, yy,tt; 

484 

485  holdVehicle.coord.x  =  vehicle.coord.x; 

486  holdVehicle.coord.y  =  vehicle.coord.y; 

487  holdVehicle.heading  =  vehicle.heading; 

488  holdVehicle.kappa  =  vehicle.kappa; 

489 

490  x  =  incrementalMotion.coord.x; 

491  y  =  incrementalMotion.coord.y; 

492  theta  =  holdVehicle.heading; 

493 

494 

495  xx  =  cos(theta)  *  x  -  sin(theta)  *  y  +  holdVehicle.coord.x; 

496  yy  =  sin(theta)  *  x  +  cos(theta)  *  y  +  holdVehicle.coord.y; 

497 

498  tt  =  holdVehicle.heading  +  incrementalMotion.heading; 

499 

500  vehicle.coord.x  =  xx; 

501  vehicle.coord.y  =  yy; 
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502  vehicle.heading  =  tt; 


503  vehicle.kappa  =  holdVehicle.kappa; 
504} 

505 


6  06  ^^^^^^^^i^hih1****^***^********^**^**^*************^**********^**/ 


507/*  FUNCTION  :  steer(robot,line)  PURPOSE :  evaluate  steering  */ 

508/*  function  */ 


510double  steer() 


511 

512{ 

513  double  lambda,  angle,  dist; 

514 

515  if  (currentPath.config.kappa  ==  0.0) 

5 1 6  lambda  =  -  currentPath.a  *  vehicle.kappa 

517  -  currentPath.b  *  norm(vehicle.heading  -  currentPath.config.heading) 

518  -  currentPath.c  *(-(vehicle.coord.x  -  currentPath.config.coord.x) 

519  *  sin(currentPath.config.heading) 

520  +(vehicle.coord.y  -  currentPath.config.coord.y) 

521  *  cos(currentPath.config.heading)); 

522  else 

523  { 
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524  angle  =  Psi(vehicle.coord,  currentPath.  center); 

525  dist  =  distance(currentPath. center,  vehicle.coord); 

526  if  (currentPath.config.kappa  >  0.0) 

527  { 

528  lambda  =  -  currentPath.a  *  (vehicle.kappa-currentPath.config.kappa) 

529  -  currentPath.b  *  norm(vehicle.heading-(angle-HPI)) 

530  -  currentPath.c  *  (currentPath. radius  -  dist); 

531  } 

532  else 

533  lambda  =  -  currentPath.a  *  (vehicle.kappa-currentPath.config.kappa) 

534  -  currentPath.b  *  norm(vehicle.heading-(angle+HPI)) 

535  -  currentPath.c  *  (currentPath.radius  +  dist); 

536  } 

537  return  lambda; 

538} 

539 

54Q/** ************************************************** **********/ 

54 1  void  constantsO 

542  { 

543  double  k; 

544 

545  k  =  1 .0/sigma; 
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546  currentPath.a  =  3.0*k; 


547  currentPath.b  =  3.0*k*k; 

548  currentPath.c  =  k*k*k; 

549} 

550/*******************************************************************/ 


551/*  Function:  Psi_function()  */ 

552/*  Purpose:  Computes  the  Psi  function  of  two  given  points  */ 

553/*  Parameters:  point  pi, p2  */ 

554/*  Returns:  double  */ 

555/*  Comments:  */ 


^  J  ^  ^  5}c  3|c  3$C  *$€  5j%  «jC  5$C  5fC  5fC  5jC  ?|C  5jC  5jC  5$C  3jC  5jC  5}C  5jc  )jc  3fC  5jC  ^  jjc  5|c  rjC  5}C  jjC  5jC  5jC  5jC  5jC  #jc  5jC  j 

557double 

558Psi(point  pi, point  p2) 

559 

560  { 

561  if  (  p2.y  -  p  1  .y  ==  0.0  &&  p2.x  -  p  1  .x  ==  0.0) 

562  return  0.0; 

563  else 

564  return  atan2(p2.y  -  pl.y,  p2.x  -  pl.x); 

565} 

566 

567 
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568/*******************************************************************/ 


569/*  Function:  distance()  */ 

570/*  Purpose:  Computes  the  distance  between  two  given  points  */ 

571/*  Parameters:  point  pi, p2  */ 

572/*  Returns:  double  */ 

573/*  Comments:  */ 


575double 

576distance(point  pi, point  p2) 

577 

578  { 

579  double  X,  Y; 

580 

581  X  =  pl.x-p2.x; 

582  Y  =  pl.y -  p2.y; 

583  return  new_sqrt(  X*X  +  Y*Y ); 

584} 

585 

586void  initTangent() 

587  { 

588  currentPath.config.coord.x  =  0.0; 

589  currentPath.config.coord.y  =  0.0; 
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590  currentPath.config.heading  =  0.0; 

591  currentPath.config.kappa  =  0.0; 

592  currentPath.radius  =  0.0; 

593  currentPath. center. x  =  0.0; 

594  currentPath.center.y  =  0.0; 

595  sigma  =  20.0; 

596  constants(); 

597 

598  motion.Speed  =  10.0; 

599  motion.Theta  =  0.0; 

600  motion.Omega  =  0.0; 

601 

602  vehicle.coord.y  =  0.0; 

603  vehicle.coord.x  =  0.0; 

604 

605 
606} 

607 
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APPENDIX  E:  SOURCE  CODE  (MOTOR.C) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 

Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /*  ============================================================= 

2  //Edward Mays 

3  //  Shpeherd  project 

4  // 20  February  1997 

5  // MotionControl 

6  //============================================*/ 

7 

8  #include  "motor.h" 

9  #include  "shepherd.h" 

10  #include  "math.h" 

11 

12  void  readEncoders()  { 

1 3  readDri  veEncoders(dri  veReadings) ; 

1 4  readSteerEncoders(steerReadings); 

15  } 

16 

17/*  */ 

1 8  /*  Verifies  validity  of  incoming  speeds/angles  and  converts  */ 

19  /*  digitial  input  for  the  DA  board  */ 

20  /*  */ 

21  void  driveMotors(){ 

22 

23  int  ix,Speed_Digit,Steer_Digit,  counter; 


131 


24  double  speed  1 ,  steer  1 ,  temp; 

25 

26  unsigned  short  bitMask=0x8000;  /*  access  bit  15  for  align  wheel  1  */ 

27  unsigned  short  *servoStatus=(unsigned  short  *)  ( VME942 1 +0x00ca) ;  /*  digital  input  */ 

28 

29  bitMask  =  bitMask  »  3; 

30 

31  /*  updateWheelDrive();  wheel  values  for  driving  */ 

32  /*  updateWheelS teer() ;  */ 

33  /*  comupte  the  current  actual  wheel  direction  in  WheelDirAct[]  */ 

34 

35  if  (mode  !=  100)  { 

36  for(ix  =0;  ix  <ARRAY_SIZE;  ix++){ 

37  ******!c****!)t*H!******steerjng/(jriving  interaction*************  */ 

38  /*  here  +/- 1/50  of  the  steering  value  is  added  to  the  driving  */ 

39  /*  for  each  specified  wheel.  Note  the  negative  sign  on  elements  [1]  */ 

40  /*  and  [3]provide  the  same  direction  driving  as  elements  [0]  and  [2]  */ 

41 

42  Omega_Speed  =  desiredSpeeds[ix]  + 

43  SteerDriveInteract*desiredAngleRates[ix]*WheelRadius;  /*  cm/sec  */ 

44 

45  /*  conversion  to  digits  */ 

46  Speed_Digit  =  velocityReferenceTable(Omega_Speed,ix)  + 

47  DriveFeedBackGain*(Omega_Speed  -  actualSpeeds[ix]); 

48  Steer_Digit  =  rateReferenceTable(desiredAngleRates[ix]) 


49 


+  steerFeedbackGain*(desiredAngleRates[ix]-actualAngleRates[ix]) 


50 


+  angleFeedbackGain*norm(desiredAngles[ix]-actualAngles[ix]); 


51 

52  if  (Speed_Digit>DigitsHigh)  /*  Limitation  */ 

53  Speed_Digit=  DigitsHigh; 

54  if  (Steer_Digit>DigitsHigh) 

55  Steer_Digit=  DigitsHigh; 

56  if  (Speed_Digit<DigitsLow) 

57  Speed_Digit=  DigitsLow; 

58  if  (Steer_Digit<DigitsLow) 

59  Steer_Digit=  DigitsLow; 

60 

61  switch(mode){ 

62  case  2: 

63  case  3: 

64  case  4: 

65  case  5: 

66  case  6: 

67  case  7: 

68  case  8: 

69  case  9: 

70  case  10: 

7 1  Speed_Digits[ix]=  (short)Speed_Digit;  /*  casting  to  short  */ 

72  Steer_Digits[ix]=  (short)Steer_Digit; 

73  break; 

74 

75  case  1: 
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76  speedl  =  Speed_Digits[ix]; 

77  steer  1  =  Steer_Digits[ix]; 

78  if  (  speedl  >  0)  speedl--; 

79  if  ( speedl  <  0)  speedl ++; 

80  if  (  steer  1  >  0)  steer  1--; 

81  if  (  steer  1  <  0)  steerl++; 

82  Speed_Digits[ix]  =  speedl; 

83  Steer_Digits[ix]  =  steerl; 

84  break; 

85  }  /*  end  switch  */ 

86  }  /*  end  for  */ 

87  }  /*  end  if  */ 

88  else  { 

89  for  (ix=0;  ix<3;  ix++){ 

90  Steer_Digits[ix]  =  0; 

91  } 

92  for  (ix=0;  ix<4;  ix++){ 

93  Speed_Digits[ix]  =  0; 

94  } 

95 

96  switch(modeTstate){ 

97  case  0: 

98  Steer_Digits[3]  =  50*Flag; 

99  modeTstate  =  1; 

100  break; 

101 
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102 

case  1: 

103 

modeTstate  = 

104 

break; 

105 

106 

case  2: 

107 

modeTstate  = 

108 

break; 

109 

110 

case  3: 

111 

modeTstate  = 

112 

break; 

113 

114 

case  4: 

115 

modeTstate  = 

116 

break; 

117 

118 

case  5: 

119 

modeTstate  = 

120 

break; 

121 

122 

case  6: 

123 

modeTstate  = 

124 

break; 

125 

126 

127 

case  7: 
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128 

modeTstate  =  8; 

129 

break; 

130 

131 

case  8: 

132 

modeTstate  =  9; 

133 

break; 

134 

135 

case  9: 

136 

modeTstate  =  10; 

137 

break; 

138 

139 

case  10: 

140 

modeTstate  =  11; 

141 

break; 

142 

143 

case  1 1 : 

144 

modeTstate  =  12; 

145 

break; 

146 

147 

case  12: 

148 

modeTstate  =  13; 

149 

break; 

150 

151 

case  13: 

152 

modeTstate  =  14; 

153 

break; 

154 


155 

156 

157 

158 

159 

160 
161 
162 

163 

164 

165 

166 

167 

168 

169 

170 

171 

172 

173 

174 

175 

176 

177 

178 


case  14: 

modeTstate  =15; 
break; 

case  15: 

modeTstate  =16; 
break; 

case  16: 

modeTstate  =17; 
break; 

case  17: 

modeTstate  =  18; 
break; 

case  18: 

modeTstate  =  19; 
break; 


case  19: 

if  (bitMask&*servoStatus)/*  read  servo  status,  */ 


{ 


/♦wait  until  wheel  aligned  */ 


179 


Flag  =  -Flag; 
modeTstate  =  20; 
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180 

} 

181 

break; 

182 

183 

case  20: 

184 

Steer_Digits[3]  = 

185 

modeTstate  =  21; 

186 

break; 

187 

188 

case  21: 

189 

modeTstate  =  22; 

190 

break; 

191 

192 

case  22: 

193 

modeTstate  =  23; 

194 

break; 

195 

196 

case  23: 

197 

modeTstate  =  24; 

198 

break; 

199 

200 

case  24: 

201 

modeTstate  =  25; 

202 

break; 

203 

204 

case  25: 

205 

modeTstate  =  26; 
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206  break; 

207 

208  case  26: 

209  modeTstate  =  27; 

210  break; 

211 

212  case  27: 

213  modeTstate  =  0; 

214  break; 

215 

216  default :  break; 

217  }  /*  end  switch  */ 

218  } /*  end  else*/ 

219 

220  driveSteer(Steer_Digits); 

221  driveSpeed(Speed_Digits); 

222  }/*  end  driveMotors  */ 

223 

224  /*  Wheel  Driving  function  */ 

225void  driveSpeed(short  Speed_Digits[])  { 
226 


227  unsigned  int  *servoControl=(unsigned  int  *)VME2170; 

228  short  *servoOutl=(unsigned  short*)(VME92 10+0x0082) 

229  short  *servoOut3=(unsigned  short*)(VME92 10+0x0086) 

230  short  *servoOut2=(unsigned  short* )(VME92 10+0x0084): 

231  short  *servoOut4=(unsigned  short*)(VME92 10+0x0088) 


/*  Data  Out  */ 

/*  Analog  out  */ 
/*  Analog  out  test*/ 
/*  Analog  out  test*/ 
/*  Analog  out  test*/ 
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232 


233  unsigned  int  wheelSelect=0x00924924;  /*  select  all  wheels  for  driving  or  steering  */ 

234 

235  *servoControl=wheelSelect; 

236 

237  *servoOutl=  (-Speed_Digits[0])«4; 

238  *servoOut3=  (-Speed_Digits[2])«4; 

239  *servoOut2=  Speed_Digits[l]  «4; 

240  *servoOut4=  Speed_Digits[3]  «4; 

241 

242  return; 

243 }  /*  driveSpeed  */ 

244 

245 

246 

247/*  Wheel  Steering  function  */ 

248void  driveSteer(short  Steer_Digits[]){ 

249 

250  unsigned  int  *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

251  short  *servoOutl=(unsigned  short*)(VME9210+0x008A);  /*  Analog  out  wheell*/ 

252  short  *servoOut3=(unsigned  short*)(VME9210+0x008E);  /*  Analog  out  wheel3*/ 

253  short  *servoOut2=(unsigned  short*)(VME9210+0x008C);  /*  Analog  out  wheel2*/ 

254  short  *servoOut4=(unsigned  short*)(VME92 10+0x0090);  /*  Analog  out  wheel4*/ 

255 

256 

257 
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258  /*  select  all  wheels  for  driving  or  turning  */ 

259  unsigned  int  wheelSelect=0x00924924; 

260 

261  *servoOutl=  Steer_Digits[0]«4;  /*  a  neg  volt  turns  wheels  clockwise  */ 

262  *servoOut3=  Steer_Digits[2]«4;  /*  a  pos  volt  turns  counter  clockwise*/ 

263  *servoOut2=  Steer_Digits[l]«4; 

264  *servoOut4=  Steer_Digits[3]«4; 

265  *servoControl=wheelSelect;  /*  turn  on  selected  servo  motor  */ 

266 

267  return; 

268 

269 }  /*  end  of  driveSteer  */ 

270 

271 

272 

273/*  Wheel  stop  function  */ 

274 void  allStop(){ 

275 

276  unsigned  int  *servoControl=(unsigned  int  *)VME2 170; /*  Data  Out  */ 

277  /*  short  *servoOutl=(unsigned  short*)(VME9210+0x0084);*/ 

278 

279  /*  deselect  all  wheels  for  driving  and/or  turning  */ 

280 

28 1  *servoControl=0x00000000;  /*  turn  off  selected  servo  motor  */ 

282  /*  *servoOutl=  0.0;*/  /*  temp,  does  not  belong  in  this  function  */ 

283  initBoardsQ; 


284  return; 

285 

286}  /*  end  of  allStop  */ 

287 

288 

289void  wheelDrive() 

290 {  int  ix,a; 

291  double  alpha[ARRAY_SIZE]={30, 0, 0,  0},  beta[ARRAY_SIZE]={0.0, 0.0, 0.0,  0.0}; 

292 

293  driveMotors(alpha,beta); 

294 

295  return; 

296}/*end  wheelDrive  */ 

297 

298 

299 

300void  readDriveEncoders(unsigned  long  int  array []) 

301  { 

302  unsigned  char  *p=(unsigned  char*)VMECTRl,  cl,  c2,  c3; 

303  int  ix; 

304  long  int  temp; 

305 

306  for  (ix=0;  ix<4;  ix++)  {  /*  read  all  four  motors  subsequently  */ 

307 

308  *(p+3)=0x03;  /*  load  output  latch  from  counter  */ 

309  *(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  */ 
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310 

311  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

312  /*  first  access  to  Output  Latch  Register  reads  least  significant  */ 

313  /*  byte  first  */ 

314 

315  cl  =  *(p+l)&0x00ff; 

316  c2  =  *(p+l)  &  OxOOff; 

317  c3  =  *(p+l)&  OxOOff; 

318  array[ix]  =  ((unsigned  int)cl)l  ((unsigned  int)c2  «  8)  I 

319  ((unsigned  int)c3  «  1 6); 

320 

321  p=p+4;  /*  increment  pointer  for  next  counter  */ 

322 

323 

324  } 

325  return; 

326 }  /*  end  of  readDriveEncoders  */ 

327 

328 

329int  readSteerEncoders(unsigned  long  int  array[]) 

330{ 

331  unsigned  char  *p=(unsigned  char*)(VMECTRl  +  0x0100),  cl,  c2,  c3; 

332  int  ix; 

333 

334 

335  for  (ix=0;  ix<4;  ix++)  {  /*  read  all  four  motors  subsequently  */ 


143 


336 

337  *(p+3)=0x03;  /*  load  output  latch  from  counter  */ 

338  *(p+3)=0x01 ;  /*  control  register,  initialize  two-bit  output  latch  */ 

339 

340 

341  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

342  /*  first  access  to  Output  Latch  Register  reads  least  significant  byte  first  */ 

343 

344  cl  =  *(p+l)  &  OxOOff; 

345  c2  =  *(p+l)  &  OxOOff; 

346  c3  =  *(p+l)  &  OxOOff; 

347  array[ix]  =  ((unsigned  int)cl)l  ((unsigned  int)c2  «  8)  I 

348  ((unsigned  int)c3  «  16); 

349 

350 

35 1  p=p+4;  /*  increment  pointer  for  next  counter  */ 

352 

353  } 

354  return; 

355 }  /*  end  of  reads teerEncoders  */ 

356 

357 void  displayDirections() 

358  { 

359  /*if  (edCounter%10  ==  0){  */ 

360  convertBCD(bcdString+9,(unsigned  int)(WheelDirDes[2]*RadsToDegrees)); 

361  /*bcdString+9*/ 
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362 


convertBCD(bcdString+9,  (unsigned  int)edCounter); 

363  bcdString[3]='0'; 

364  bcdString[4]='3'; 

365  bcdString[6]='4'; 

366  bcdString[7]-0'; 

367  sioOut(0, bcdString); 

368 

369  /*  sioOut(0,clrLine);  */  /*  clear  line  */ 

370 

37 1  convertBCD(bcdString+9,  (unsigned  int)(WheelDirAct[2]*RadsToDegrees)); 

372  bcdString[3]='0'; 

373  bcdString[4]='3'; 

374  bcdString[6]='6'; 

375  bcdString[7]='0'; 

376  sioOut(0, bcdString); 

377 
378} 

379 

380/*  2  May  */ 

381  void  displaySpeed() 

382  { 

383 

384  convertBCD(bcdString+9,(unsigned  int)  1 ); 

385  bcdString[3]='0'; 

386  bcdString[4]=’3’; 

387  bcdString[6]='4'; 
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388  bcdString[7]='0'; 

389  sioOut(0, bcdString); 

390 

391  /*  sioOut(0,clrLine);  *//*  clear  line  */ 

392 

393  convertBCD(bcdString+9,(unsigned  int)steerReadings[l]); 

394  bcdString[3]='0'; 

395  bcdString[4]='3'; 

396  bcdString[6]='6'; 

397  bcdString[7]='0'; 

398  sioOut(0, bcdString); 

399) 

400 

40 1  void  testDri  ve  1  () 

402  { 

403  desiredAngleRates[0]  =  1; 

404  desiredAngleRates[l]  =  1; 

405  desiredAngleRates[2]  =  1 ; 

406  desired  An  gleRates  [3]  =  1; 

407  desiredSpeeds[0]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

408  desiredSpeedsf  1  ]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

409  desiredSpeeds[2]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

410  desiredSpeeds[3]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

411 
412} 

413 
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414/*  2  May  */ 

415void  testDrive() 

416  { 

417  double  MM,  RES,  N=  1044548,  C=0.001; 

418 


419sioOut(0,"Entering  testDrive  ...\n\r"); 

420 

421 


422if(20<(N  -  steerReadings[l])*C){ 

423 

424  MM=20.0; 

425  } 

426  else{ 


427 

428  MM=(N  -  steerReadings[l])*C; 

429  } 

430 

431 

432  if(MM>0){ 

433 

434  RES=MM; 

435  J 


436  else{ 

437 

438  RES=0.0; 

439  } 
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440 


441 

442 

443  /*  RES=max(min(20,(N  -  steerReadings[0])*C),0);  *1 

444 

445sioOut(0, "Leaving  testDrive  ...\n\r"); 

446  desiredSpeeds[l]  =  -(RES);  /*  wheels  2&4  must  have  minus  sign  */ 

447 

448  return; 

449} 

450 

451 

452 

453 

454 

455void  computeActualRates() 

456  { 

457 

458int  i; 

459double  count,speed; 

460 

461  for(i=0;  i<=3;  i++) 

462  { 

463  if(PreviousCountSpeed[i]  =  99999999)  /*  for  derivative  for  speed  */ 

464  actualSpeeds[i]=  0.0; 

465  else 
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466  actualSpeeds[i]= 

467  (convertDifference((driveReadings[i]  -  PreviousCountSpeed[i])) 

468  *DigitToCmDrive[i])/DeltaT; 

469  PreviousCountSpeed[i]  =  driveReadings[i]; 

470 

471  if(PreviousCountSteer[i]  ==  99999999)  /*  for  derivative  for  steering  */ 

472  actual AngleRates[i]=  0.0; 

473  else 

474  actualAngleRates[i]= 

475  (convertDifference((steerReadings[i]  -  PreviousCountSteer[i])) 

476  *digitToRadSteer)/DeltaT; 

477  PreviousCountSteer[i]  =  steerReadings[i]; 

478  } 

479} 

480 

481 

482 

483void  accumulateDriveSpeed() 

484  { 

485  int  i; 

486 

487for(i=0;i<=3;i++){ 

488  Display_Speeds[i]  +=  actualSpeeds[i]; 

489  } 

490  return; 

491} 
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492 


493 void  accumulateDriveSteer() 

494  { 

495  int  i; 

496 

497 for(i=0;i<=3  ;i++)  { 

498  Display_Steers[i]  +=  10*actualAngleRates[i]; 

499  actual  Angles  [i]  +=  actualAngleRates[i]*DeltaT; 
500} 

501  return; 

502} 

503 

504/*  added  15  may  */ 

505 void  displayDriveAngle() 

506  { 

507 

508  double  angle,  anglel,  angle2,  angle3; 

509  angle  =  actual AngleRates[0]  *  1000.0; 

510  anglel  =  actual AngleRates[l]  *  1000.0; 

511  angle2  =  actual AngleRates[2]  *  1000.0; 

512  angle3  =  actualAngleRates[3]  *  1000.0; 

513 

514  if  (edCounter%  1 00  =  0){ 

515  convertInt(bcdString+9,(int)desiredAngleRates[0]); 

516  bcdString[3]=’0'; 

517  bcdString[4]=’3'; 
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518  bcdString[6]='4'; 

519  bcdString[7]='0'; 

520  sioOut(0,bcdString); 

521  convertInt(bcdString+9,(int)  angle); 

522  bcdString[3]-0'; 

523  bcdString[4]='3'; 

524  bcdString[6]='6'; 

525  bcdString[7]-0'; 

526  sioOut(0,bcdString); 

527  convertInt(bcdString+9,(int)  angle  1); 

528  bcdString[3]='0'; 

529  bcdString[4]=4'; 

530  bcdString[6]='6'; 

531  bcdString[7]='0'; 

532  sioOut(0,bcdString); 

533  convertInt(bcdString+9,(int)  angle2); 

534  bcdString[3]='0'; 

535  bcdString[4]='5'; 

536  bcdString[6]='6'; 

537  bcdString[7]='0’; 

538  sioOut(0,bcdString); 

539  convertInt(bcdString+9,(int)  angle3); 

540  bcdString[3]=’0'; 

541  bcdString[4]='6'; 

542  bcdString[6]=’6'; 

543  bcdString[7]='0'; 


544  sioOut(0,  bcdString) ; } 

545 

546  return; 

547} 

548 

549 

550 

551  double  velocityReferenceTable(double  desiredVelocity,int  i) 

552  { 

553  double  inVelocity, 

554  outVelocity; 

555 

556  inVelocity=new_abs(desiredVelocity); 

557 

558  if  (inVelocity>=0.0  &&  inVelocity<=5.0) 

559  outVelocity  =  inVelocity*Kl  [i]; 

560 

561  if  (inVelocity>5.0  &&  inVelocity<  8.0) 

562  outVelocity  =  inVelocity*K2[i]; 

563 

564  if  (inVelocity>=8.0  &&  inVelocity<20.0) 

565  outVelocity  =  inVelocity*K3[i]; 

566 

567  if  (inVelocity>=20.0  &&  inVelocity<=  70.0) 

568  outVelocity  =  inVeIocity*K4[i]; 

569 
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570  if  (inVelocity>70.0  &&  in  Velocity  <K5) 

571  outVelocity  =  in  Velocity*K6[i] ; 

572 

573  if  (inVelocity>  K5) 

574  outVelocity=1023; 

575 

576  if  (desiredVelocity<  0.0) 

577  outVelocity  =  -  outVelocity; 

578 

579  return  outVelocity; 

580)  /*  end  velocityLookupTable  */ 

581 

582 

583double  rateReferenceTable(double  desiredRate) 

584  { 

585  double  inRate, 

586  outDigit; 

587 

588  /*outDigit  =  new_abs(desiredRate);  *//*  test  only  */ 

589 

590  inRate=new_abs(desiredRate); 

591 

592  if  (inRate<=  5.234) 

593  outDigit  =  inRate*  1 95.4 1 55  ; 

594  else 

595  outDigit=1023; 
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596 


if  (desiredRate<  0.0) 
outDigit  =  -  outDigit; 


597 

598 

599 

600 

601  return  outDigit; 
602} 

603 

604 

605 


606/*************************************************************** 


607  Function  convertDifference()  returns  the  difference  between  the  new  shaft 

608  encoder  position  and  the  old  shaft  encoder  position.  The  shaft  encoder  values 

609  contain  only  24  bits  (OxOOOOOO-Oxffffff).  The  routine  adjusts  for  the  trans- 

610  ition  from  Oxffffff  to  0x000000  and  vice  versa. 


g  J  J  ****************************************************************/ 


612 

61 3int  convertDifference(int  value) 
614{ 

6 1 5  if(value  <  -0x800000) 

616  value  &=  OxOOffffff; 

617  else  if(value  >=  0x800000) 

618  value  1=  OxffOOOOOO; 

619 


620  return  value; 
621} 
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622 


623/* - * 

624  *  * 

625  *  File:  S  E  R  V  O  .  C  * 

626  *  * 

627  *  Environment:  GCC  Compiler  v2.7.2  * 

628  *  Last  update:  30  January  1997  * 

629  *  Name:  Thorsten  Leonardy  * 

630  *  Purpose:  Provides  the  kernel  for  SHEPHERD.  * 

631  *  * 

632  *  Compiled:  >gcc  -c  -m68040  -o  servo.o  servo.c  * 

633  *  * 

634  * - */ 

635 

636 

637 

638/* - * 

639  *  readWheelStatus()  * 

640  *  * 

641  *  Environment:  GCC  Compiler  v2.7.2  * 

642  *  Last  update:  20  February  1997  * 

643  *  Name:  Thorsten  Leonardy  * 

644  *  Purpose:  This  function  reads  the  wheels  counter  status.  * 

645  *  This  routine  makes  use  of  the  fact  that  arrays  are  stored  * 

646  *  in  memory  consecutively.  * 

647  *  * 
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648  *  array  points  to  the  beginning  of  the  array  'wheelEncoder'. 

649  * - */ 


650void  readWheelStatus(unsigned  char  *array) 

651  { 

652  unsigned  char  *p=(unsigned  char*) VMECTR 1 ; 

653  int  ix; 

654 

655  for  (ix=0;  ix<8;  ix++)  {  /*  read  all  eight  motors  subsequently  */ 

656 

657  *(p+3)=0x03;  /*  load  output  latch  from  counter  */ 

658  *(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  */ 

659 

660  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

661  /*  first  access  to  Output  Latch  Register  reads  least  significant  byte  first  */ 

662  *(array+3)=*(p+l); 

663  *(array+2)=*(p+l); 

664  *(array+l)=*(p+l); 

665  *(array+0)=0; 

666 

667  array+=4;  /*  point  to  next  entry  in  wheelEncoder*/ 

668  p=p+4;  /*  increment  pointer  for  next  counter  */ 

669 

670  if  (ix=3)  p=(unsigned  char*)VMECTR2;  /*  access  the  second  VME  Counter  */ 

671 

672  } 

673  return; 
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674 }  /*  end  of  readWheel  Status  *1 

675 

676 

677/* - * 

678  *  clearShaftEncoder(unsigned  short  motors)  * 

679  *  * 

680  *  Environment:  GCC  Compiler  v2.7.2 

681  *  Last  update:  04  March  1997  * 

682  *  Name:  Thorsten  Leonardy  * 

683  *  Purpose:  This  function  clears  the  selected  shaft  encoder.  * 


684  *  * 

685  *  motors  bit  mask  to  select  motors,  eg.  0x042  selects  motor  2  and  7  * 

686  *  to  be  cleared.  * 

687  * - */ 


688void  clearShaftEncoder(unsigned  short  motors) 

689  { 

690  unsigned  char  *p=(unsigned  char*)VMECTRl ; 

691  intix; 

692 

693  for  (ix=0;  ix<8;  ix++,motors/=2)  { 

694  if  (motors  &  0x01)  *(p+3)=0x04;  /*  clear  respective  counter  */ 

695  p=p+4;  /*  access  next  pointer  */ 

696  if  (ix=3)  p=(unsigned  char*)VMECTR2;  /*  access  the  second  VME  Counter  */ 

697  ) 

698  return; 

699 }  /*  end  of  clearShaftEncoder  */ 
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700 

701 

702/* - 

703  *  align() 

704  *  Environment:  GCC  Compiler 

705  *  Last  update:  07  August  1997  m 


706  *  Name:  Thorsten  Leonardy  ,Yutaka  Kanayama,  Ed  Mays  * 

707  *  Purpose:  This  function  will  align  SHEPHERD'S  wheels  such  that  all  * 

708  *  will  point  in  the  forward  direction.  It  utilizes  the  hall  * 

709  *  sensors  for  each  of  the  four  wheels.  Crucial  parameters  * 

710*  are  as  follows:  * 

711  *  * 

712  * - */ 


713void  align(void) 

714{ 

715  unsigned  int  *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

716  unsigned  short  *servoOut=(unsigned  short*)(VME92 10+0x008 A);  /*  Analog  out  */ 

717  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 

718  unsigned  short  bitMask=0x8000,  bitMaskl;  /*  access  bit  15  for  align  wheel  1  */ 

719  unsigned  int  wheel Select=0x00004000;  /*  select  servo  for  turning  wheel  1  */ 

720  int  ix,  notYet;  /*  just  a  counter  */ 

721 

722  do  { 

723  notYet  =  0; 

724  bitMaskl  =  bitMask; 

725  for  (ix=0;  ix  <  4;  ix++) 
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726  { 

727  if  (bitMaskl  &  *servoStatus) 

728  { 

729  Steer_Digits[ix]  =  0; 

730  } 

731  else 

732  { 

733  Steer_Digits[ix]  =  40; 

734  notYet++; 

735  } 

736  bitMaskl  =  bitMaskl  »  1;  /*  select  next  status  align  bit  */ 

737  } 

738  driveSteer(Steer_Digits); 

739  }  while(notYet); 

740  *servoControl=0x00000000;  /*  disable  all  wheels  */ 

741  return; 

742 }  /*  end  of  align  */ 

743 

744/* - * 

745  *  alignAfterRotate()  * 

746  *  Environment:  GCC  Compiler  * 

747  *  Last  update:  07  August  1997  m  * 

748  *  Name:  Thorsten  Leonardy.Yutaka  Kanayama,  and  Ed  Mays  * 

749  *  Purpose:  This  function  will  align  SHEPHERD'S  wheels  such  that  all  * 

750  *  will  point  in  the  forward  direction.  It  utilizes  the  hall  * 

751*  sensors  for  each  of  the  four  wheels.  Crucial  parameters  * 
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752  * 


are  as  follows: 


753  *  * 

754  * - */ 

755void  alignAfterRotate(void) 

756  { 

757  unsigned  int  *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out*/ 

758  unsigned  short  *servoOut=(unsigned  short*)(VME92 10+0x008 A);  /*  Analog  out  */ 

759  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 

760  unsigned  short  bitMask=0x8000,  bitMaskl;  /*  access  bit  15  for  align  wheel  1  */ 

761  unsigned  int  wheelSelect=Ox00004000;  /*  select  servo  for  turning  wheel  1  */ 

762  int  ix,  notYet;  /*  just  a  counter  */ 

763 

764  do  { 

765  notYet  =  0; 

766  bitMaskl  =  bitMask; 

767  for  (ix=0;  ix  <  4;  ix++) 

768  { 

769  if  (bitMaskl  &  *servoStatus) 

770  { 

771  Steer_Digits[ix]  =  0; 

772  } 

773  else 

774  { 

775  if(  ix==l  II  ix=2  ) 

776  Steer_Digits[ix]  =  40;  /*  for  wheel  1  and  2,  rotate  CCW  */ 

777  else 
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778  Steer_Digits[ix]  =  -40;  /*  for  wheel  0  and  3,  rotate  CW  */ 

779  notYet++; 

780  } 

781  bitMaskl  =  bitMaskl  »  1;  /*  select  next  status  align  bit  */ 

782  } 

783  driveSteer(Steer_Digits); 

784  }  while(notYet); 

785  *servoControl=0x00000000;  /*  disable  all  wheels  */ 

786  return; 

787 }  /*  end  of  align  */ 

788 

789 

790/* - * 

791  *  align Wheels()  * 

792  *  Environment:  GCC  Compiler  * 

793  *  Last  update:  07  January  1997  * 

794  *  Name:  Thorsten  Leonardy  * 

795  *  Purpose:  This  function  will  align  SHEPHERD'S  wheels  such  that  all  * 

796  *  will  point  in  the  forward  direction.  It  utilizes  the  hall  * 

797  *  sensors  for  each  of  the  four  wheels.  Crucial  parameters  * 

798  *  are  as  follows:  * 

799  *  * 

800  *  servoControl  Base  address  for  the  channels  controling  the  servo  motors  * 

801  *  switch  servos  on  an  off  by  accessing  this  address.  * 

802  *  Each  servo  is  controlled  by  three  bits:  * 

803  *  bits  0..2  ->  driving  wheel  1  * 
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804  * 

805  * 
806* 

807  * 

808  * 
809* 
810* 
811  * 
812* 


3. . 5  ->  driving  wheel  2 

6..  8  ->  driving  wheel  3 

9..  11  ->  driving  wheel  4 

12.. 14->  turning  wheel  1 

15..  17  ->  turning  wheel  2 

18. . 20  ->  turning  wheel  3 

21..  23  ->  turning  wheel  4 

24.. 31  ->  not  used 


8 1 3  *  servoOut  Base  address  for  the  analog  output  card  controlling  the 

8 14  *  speed  of  the  servos.  Only  the  highest  12  bits  are  used.  * 

8 1 5  *  0x00 1 0  ->  selects  lowest  positive  velocity  * 

8 16  *  0x7ff0  ->  selects  highest  positive  velocity  * 

817  *  OxfffO  ->  selects  lowest  negative  velocity  (i.e  -1  m/s)  * 

818  *  0x8000  ->  selects  highest  negative  velocity  (i.e.  -lOOOm/s) 

819*  * 

820  *  It  has  been  found  that  the  MSB  does  not  work  properly. 

821  *  Therefore,  the  velocities  should  lie  within  1 1  bit  range,  * 

822*  -1024  <=  velocity  <=  +1023  * 

823  *  * 

824  *  servoStatus  Base  address  for  reading  the  servo  status  * 


825  * 

The  alignment  bits  are:  Port  B,  Bit  15  for  wheel  1 

* 

826* 

Bit  14  for  wheel  2  * 

827  * 

Bit  13  for  wheel  3  * 

* 


828  * 


Bit  12  for  wheel  4 


830  * 


*/ 

831 

832 

833void  align  Wheels(void) 

834  { 

835  unsigned  int  *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

836  unsigned  short  *servoOut=(unsigned  short*)(VME9210+0x008A);  /*  Analog  out  */ 

837  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 

838 

839  unsigned  short  bitMask=0x8000;  /*  access  bit  15  for  align  wheel  1  */ 

840  unsigned  int  wheelSelect=0x00004000;  /*  select  servo  for  turning  wheel  1  */ 


841  int  wheel;  /*  just  a  counter  */ 

842 

843  /* - * 

844  *  align  wheels  subsequently,  start  with  wheel  1  (front  right)  * 

845  * - */ 


846  for  (wheel=l;  wheel <5;  wheel++)  { 

847 


848  *servo0ut++=0x0200;  /*  set  output  value  for  servo  first  */ 

849  /*  0x0010  corresponds  to  smallest  velocity  */ 

850  *servoControl=wheelSelect;  /*  turn  on  selected  servo  motor  */ 

851  while(!(bitMask&*servoStatus));  /*  read  servo  status,  wait  until  wheel  aligned  */ 

852  wheelSelect=  wheelSelect«3;  /*  select  next  servo  (motor)  */ 

853  bitMask  =  bitMask  »  1 ;  /*  select  next  status  align  bit  */ 

854  } 

855 
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856  *servoControl=OxOOOOOOOO; 

857 


/*  disable  all  wheels 


*/ 

858  /*  clearShaftEncoder(OxOff);  */  /*  clear  all  shaft  encoders  */ 

859 

860  /*  sioOut(0, "aligned */  /*  Output  Message  */ 

861  return; 

862 }  /*  end  of  align  Wheels  */ 

863 

864 

865 

866 

867  End  of  servo.c 

868 

869 
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1  /*  — 

2  * 

3  *  File: 


APPENDIX  F:  SOURCE  CODE  (TIMER.C) 


* 

* 


3 

*  File:  TIMER  .C 

* 

4 

* 

* 

5 

*  Environment:  GCC  Compiler  v2.7.2 

* 

6 

*  Last  update:  29  January  1997 

* 

7 

*  Name:  Thorsten  Leonardy 

* 

8 

*  Purpose:  Provides  routines  related  to  the  AM95 13  Timer  Circuit,  such  * 

9 

*  as  interrupt  initialization. 

* 

10 

*  Compiled:  >gcc  -c  -m68040  -o  timer.o  timer.c 

* 

11 

* 

* 

12 

* 

- */ 

13 

14 

15 

#include  "shepherd.h" 

16 

#include  "timer.h" 

17 

18 

19 

void  timerStart(void) 

20 

{ 

21 

22 

long  *vadr; 

23 

unsigned  char  *p; 

24 

short  *ctrlPort  =  (short*)  TIMER_CTRL; 

25 

short  *dataPort  =  (short*)  TIMER_DATA; 

26 

27 

/*  initialize  the  interrupt  counter  */ 

28 

intCounter=0; 

29 

30  /*  load  address  for  interrupt  service  routine  */ 

3 1  vadr=(long*)VB  A_TIMER; 

32  *vadr=(long)TimerHandler; 

33 

34  /*  Issue  commands  to  set  control  and  data  register  */ 

35  /*  refer  to  Fig  1-20,  1-8,  1-12  */ 

36 

37  *ctrlPort=Oxffff;  /*  Master  reset,  clear  data  registers  */ 

38  *ctrlPort=0xff5f;  /*  load  all  counters  */ 

39  *ctrlPort=Oxffef;  /*  Set  MM13  (Enter  16-bit  bus  mode)  */ 

40 

41  *ctrlPort=0xffl7;  /*  Select  master  mode  register  */ 

42  *dataPort=0xale0;  /*  set  master  mode  register ...  */ 

43  /*  + - >  f  ==  1  sec  interrupt  interval  */ 

44  /*  e  ==  0. 1  sec  */ 

45  /*  d==  0.01  sec  */ 

46  /*  c  ==  0.001  sec  */ 

47 

48  *ctrlPort=0xff05;  /*  Select  CMR  timer  5  ...  */ 

49  /*  utilize  Data  Pointer  Sequencing  */ 

50 

5 1  *dataPort=0x0e32;  /*  and  write  to  counter  mode  register  */ 

52  /*  + - >  f=  10000  */ 

53  /*  multiply  value  according  to  dataPort  below  e=  1000  */ 

54  /*  by  the  factor  set  here  to  obtain  timing...  d=  100  */ 

55  /*  c=  10  */ 

56  /*  b  =  1  */ 

57 
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58  /*  to  obtain  the  correct  timing,  multiply  value  determined  in  data-*/ 

59  /*  port  below  by  the  factor  given  above.  E.g.  dataPort  is  set  to  */ 

60  /*  58  (corresponding  to  lOusec)  and  factor  1000  is  chosen  above,  */ 

61  /*  then  the  interrupt  would  occur  every  10msec!  */ 

62 

63  *dataPort=58;  /*  load  register,  58  ->  lOusec  */ 

64  /*  930  ->  1msec  */ 

65 

66  *ctrlPort=0xff70;  /*  load  and  arm  timer  5  */ 

67 

68  p=(unsigned  char*)ISM_TIMER;  /*  ISM  Configuration  for  Timer  */ 

69  *p=0xcb;  /*  assert  LIRQ-3  to  VIC  */ 

70 

7 1  p=(unsigned  char*)VIC_LIRQ3 ;  /*  VIC  LICR  for  LIRQ-3  from  ISM  */ 

72  *p=0x03;  /*  assert  IRQ-3  from  VIC  to  68040  */ 

73 

74  return; 

75  }  /*  end  of  timerS  tart  */ 

76 

77 

79  Assembler  routines 

80 
81 

82  /*  TimerHandler,  its  address  is  set  from  within  timerStart  */ 

83 

84  asm(" 

85  .even 

86  .text 
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.globl  _TimerHandler 


87 

88 

89  _TimerHandler: 

90 

91 

92  link  a6,#-184  /*  alocate  184  Bytes  on  stack  to  save  registers  */ 

93  fsave  a6@(-184) 

94  fmovemx  fp0-fp7,sp@-  /*  move  floating  point  registers  80  bit  each  */ 

95  fmovel  fpcr,sp@-  /*  move  floating  point  Control  Regioster  */ 

96  fmovel  fpsr,sp@-  /*  move  floating  point  status  register  */ 

97  fmovel  fpiar,sp@-  /*  move  floating  point  Instruction  address  register  */ 

98  moveml  dO-d7/aO-a5,sp@-  /*  save  data  and  address  registers  (14*4  Byte)  */ 

99 

100 

101  addq.l  #0x0 1,  JntCounter /*  increment  interrupt  counter  */ 

102  move.w  #0xffe5 ,0xfff41 002  /*  clear  toggle  out  for  timer  5  */ 

103 

104  moved  #0xffff0081,al  /*  load  VME9421  Status  register  */ 

105  eor.b  #0x02, (al)  /*  toggle  green  indicator  light  to  indicate  timer  */ 

106  /*  for  interrupt  handling  is  working  properly  ...  */ 

107  and.b  #0xfe,(al)  /*  turn  red  light  on  to  indicate  that  motion  control*/ 

108  /*  will  start  (this  will  assert  the  SYSFAIL  line  on  */ 

109  /*  the  VME-Bus,  but  we  don't  care  at  this  point).  */ 

110 

111  jsr  _driver  /*  execute  motion  control  part  */ 

112 

113 

1 14  moved  #0xffff0081,al  /*  load  VME9421  (digital  out  board)  Status  register  */ 

1 1 5  or.b  #0x01  ,(al)  /*  turn  off  red  indicator  light  to  indicate  that  */ 
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116  /*  motion  control  is  done.  */ 

117 

118  moveml  sp@+,d0-d7/a0-a5 

119  fmovel  sp  @ +,fpiar 

120  fmovel  sp@+,fpsr 

121  fmovel  sp@+,fpcr 

122  fmovemx  sp@+,fp0-fp7 

123  frestore  a6  @  (- 1 84) 

124  unlk  a6 

125 

126  rte 

127  "); 

128 
129 

131  Endoftimer.c 

133 
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APPENDIX  G:  SOURCE  CODE  (MATH.C) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /* - - - * 

2  *  * 

3  *  File:  MATH.C  * 

4  *  * 

5  *  Environment:  GCC  Compiler  v2.7.2  * 

6  *  Last  update:  17  March  1997  * 


7  *  Name:  Thorsten  Leonardy  * 

8  *  Purpose:  A  Simple  Math  library.  * 

9  * _ */ 

10 


1 1  #include  "shepherd.h" 

12  #include  "math.h" 

13 

14  #define  pio4  0.785398163 

1 5  #define  pio2  1 .570796327 

16  #define  pi  3.141592654 

17  #define  pi2  6.283185307 

18 
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19 


20 

21 

22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 


/********************************************************************** 


FUNCTION:  norm() 

PARAMETERS:  double  angle  —  the  angle  to  normalize 
PURPOSE:  normalize  the  input  angle  between  -PI  and  PI 
RETURNS:  double:  the  normalized  angle  in  radians 

COMMENTS:  This  is  the  most  common  normalizing  function  used  in  the  system 
This  performs  that  same  as  norm()  and  normalize)()  in  MML10. 


double  norm(double  angle) 

{ 

while  ((angle  >  pi)  II  (angle  <=  -pi)) 

{ 

if  (angle  >  pi) 
angle  -=  pi2; 
else 

angle  +=  pi2; 

} 

return  angle; 

} 
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41 


42 

43 

44  /* - * 

45  *  * 

46  *  new_abs()  * 

47  *  * 

48  *  Environment:  GCC  Compiler  v2.7.2  * 

49  *  Last  update:  14  March  1997(mod  2  April  97  by  Ed  Mays)  * 

50  *  Name:  Thorsten  Leonardy  * 

51  *  Purpose:  A  function  returning  the  absolute  value  of  x.  * 

52  * - */ 

53  double  new_abs(double  x) 

54  { 

55  if  (x>=0.0) 

56  return  (x); 

57  else 

58  retum(-x); 

59} 

60 

61 

62 
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63 


64  /* - 

65  *  atan2() 

66  * 

67  *  Environment:  GCC  Compiler  v2.7.2 

68  *  Last  update:  17  March  1997 

69  *  Name:  Thorsten  Leonardy 

70  *  Purpose:  Computes  tan(y/x)  where  x,y  are  real.  If  both  variables  are  * 

71  *  zero,  atan2  returns  zero.  For  any  other  values,  atan2  will  * 


72 

* 

return  the  positive  angle  for  the  (x,y)-pair,  e.g. 

* 

73 

* 

(x,y)=(0,-l)  would  return  atan2=3/2*pi  ! 

* 

74 

* 

ix  determines  the  accuracy  (highest  order  term 

in  expansion) 

75 

* 

For  the  worst  case,  ly/xl  close  to  one,  ix  should  be  very  * 

76 

* 

high.  Here  is  some  data: 

* 

77 

* 

Ix/yl  ix 

accuracy  of  result  [rad] 

* 

78 

* 

0.9  101 

+-  1.88*10E-7 

* 

79 

* 

1001 

+-  1.57*10E-49 

* 

80 

* 

0.99  101 

+-  3.45*10E-3 

* 

81 

* 

1001 

+-  4.18*10E-8 

* 

82 

* 

10001 

+-  2.18*10E-48 

* 

83 

* 

0.999  101 

+-  8.76*10E-3 

* 

84 

* 

1001 

+-  3.65*10E-4 

* 

* 

* 

* 

* 

* 
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85  * 


10001  +-  4.50*10E-9 


86  * - */ 

87  double  atan2(double  y,  double  x) 

88  { 

89  double  erg=0.0,  z=0.0,  z2; 

90  int  ix=101,  flagl=0,flag2=0; 

91 

92  if  ((new_abs(y)>new_abs(x))&&(y!=0)) 

93  { 

94  z=x/y;  /*  in  case  ly/xl>l  compute  atan(l/z)  */ 

95  flagl=(y>0)-(y<0);  /*  a  handy  sign-function  */ 

96  } 

97  else  if  (x!=0) 

98  { 

99  z=y/x;  /*  in  case  ly/xkl  compute  atan(z)  */ 

100  flag2=(x<0.0);  /*  in  this  case  need  to  add  pi  to  final  result  */ 

101  } 

102 

103  /*  From  here  on  Izl  must  always  be  less  than  one  !!!  */ 

104  z2=z*z; 

105 

106  /*  Taylor  expansion  */ 
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107  if  (new_abs(z)<1.0)  {  /*  computation  for  ly/xkl  */ 

108  while  (ix>l)  { 


109  erg=z2*(l  .0/ix-erg);  /*  try  alternatively  for  accuracy:  (z2/ix)*(l  .0-ix*erg)  */ 

110  ix-=2; 

111  } 

112  erg=z-z*erg; 

113  } 

1 14  else  erg=((z>0.0)-(z<0.0))*pio4;  /*  for  ly/xl=l  result  is  either  +-  pi/4  */ 

115 

116  if  (flagl==l)  erg=pio2-erg;  /*  point  lies  in  3rd  or  4th  octant  for  flagl=+l  */ 

1 17  else  if  (flagl==-l)  erg=-pio2-erg;  /*  ...  or  in  6th  or  7th  octant  for  flag  1  =- 1  */ 

118  if  (flag2)  erg=erg+pi;  /*  point  lies  in  4th  or  5th  octant  */ 

119  /*  if  (ergcO.O)  erg=erg+pi2;  deleted  6/27/97  */ 

120 

121  retum(erg); 

122} 

123 

124 

125/* - - - * 

1 26  *  atan()  yk  * 

127* - */ 

128double  atan(double  x) 
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129  { 

1 30  double  erg=0.0,  z=0.0,  z2; 

131  int  ix=  101,  flag=0; 

132 

133  if  (x  ==  0.0) 

134  return  (0.0); 

135  if  (new_abs(new_abs(x)-1.0)  <  0.00001) 

136  return  (pio4  *  x);  /*  return  +-  pi/4  */ 

137  if  (new_abs(x)  >  1.0) 

138  { 

139  z=  1 .0/x;  /*  in  case  lxl>  1  compute  atan(  1  /x)  */ 

140  flag=(x>0)-(x<0);  /*  a  handy  sign-function  */ 

141  } 

142  else 

143  z=x;  /*  in  case  Ixkl  compute  atan(x)  */ 

144  z2=z*z;  /*  From  here  on  Izl  is  less  than  one  !!!  */ 

145  /*  Taylor  expansion  */ 

146  while  (ix>l) 

147  { 

148  erg=z2*(  1 .0/ix-erg); 

149  ix-=2; 

150  } 
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151 


erg=z-z*erg; 

152  if  (flag  ==  1)  erg  =  pio2-erg; 

153  if  (flag  ==  -1)  erg  =-pio2-erg; 

154  retum(erg); 

155} 

156 

157 

158 

159/* - 

1 60  *  cos() 

161  * 

162  *  Environment:  GCC  Compiler  v2.7.2 

163  *  Last  update:  17  March  1997 


164  *  Name:  Thorsten  Leonardy  * 

1 65  *  Purpose:  Computes  cos(x)  where  x  can  be  any  real  number. 

166*  ix  determines  the  accuracy  (highest  order  term  in  expansion)* 

167  * - */ 


1 68double  cos(double  x) 

1 69  { 

1 70  double  erg; 

171  int  quadrant,  ix=20;  /*  ix  must  be  an  even  number  */ 

172 
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173  /*  analyze  and  reduce  x  to  the  appropriate  range  ...  */ 

174  quadrant=(x/pio2+(x>=0)-(x<0))/2;  /*  determine  in  what  sector  x  is  */ 

175  x=x-quadrant*pi;  /*  reduce  x  to  region  [-pi/2... pi/2]*/ 

176  x=x*x;  /*  compute  xA2  and  store  in  x  */ 

177  erg=1.0; 

178 

179  /*  the  cosine  taylor  computation  is  a  one-liner ;-)  */ 

180  while  (ix>0)  { 

181  erg=1.0-erg*x/ix/(ix-l); 

182  ix-=2; 

183  } 

184 

185  /*  shift  sign  if  quadrant  is  not  1,3,5,...  */ 

186  if  (quadrant%2)  erg=-erg; 

187 

188  retum(erg); 

189} 

190 

191 

192/* - * 

193  *  sin()  * 

194  *  * 
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195  *  Environment:  GCC  Compiler  v2.7.2 

196  *  Last  update:  14  March  1997 


197*  Name:  Thorsten  Leonardy  * 

198  *  Purpose:  Computes  sin(x)  where  x  can  be  any  real  number.  * 

199  * - */ 


200double  sin(double  x) 

201  { 

202  retum(cos(x-pio2));  /*  since  sin(x)=cos(x-pi/2)  */ 

203} 

204 

205 

206 

207/* - * 

208  *  sqrt()  * 

209  *  Ed  Mays  and  Ferdinand  Reid  March  1997  * 

210  *  Environment:  GCC  Compiler  v2.7.2  * 

2H  * - */ 

212double  new_sqrt(double  x) 

213  { 

214  double  xl,x2; 

215  int  count; 

216 
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218  if  (x  ==  1.0)  retum(l.O); 

219  xl  =  1.0; 

220  for  (count=0;  count  <  10;  count++){ 

221  x2  =  .5  *  (xl  +  x/xl); 

222  xl  =  x2; 

223  } 

224 

225  return  (x2); 

226  } 

227 

228 

229 

230 

231/*  - 

232  *  new_sqrtl() 

233  *  Ed  Mays  and  Kanayama 

234  *  Environment:  GCC  Compiler  v2.7.2 

235  * - 

236double  new_sqrtl  (double  x) 

237  { 


238  double xl,x2; 


239  xl  =  1.0; 


240  x2=  -1.0; 

241  while(new_abs(xl-x2)  <  1.0e-9) 

242  { 

243  x2  =  xl; 

244  xl  =  .5  *(x2  +  x/x2); 

245  } 

246 

247  return  (xl); 

248  } 

249 

250 

251 

252 

253/*  ed  move  to  math.c*/ 

254double  min  (double  a,  double  b) 

255  { 

256  if  (a  <=  b) 

257  return  a; 

258  else 

259  return  b; 

260} 
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261 


262/*  ed  move  to  math.c*/ 

263 double  max  (double  a,  double  b) 


264  { 

265  if  (a>=b) 

266  return  a; 

267  else 

268  return  b; 
269} 

270/*ed*/ 

271 

272 


273 

274 

275 

276 

277 


278  End  of  math.c 

279 


183 


184 


APPENDIX  H:  SOURCE  CODE  (UTILS.C) 


1  /*  - - * 

2  *  * 

3  *  FILE:  UTILS  .C  * 

4  *  * 

5  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

6  *  LAST  UPDATE:  03  FEBRUARY  1997  * 

7  *  NAME:  THORSTEN  LEONARDY  * 

8  *  PURPOSE:  PROVIDES  THE  UTILITY  FUNCTIONS  FOR 

PROGRAM  SHEPHERD.  * 

9  *  * 

10  *  COMPILED:  >GCC  -C  -M68040  -O  UTILS.O  UTILS.C  * 

11  *  * 

12  * - */ 

13 

14  #INCLUDE  "SHEPHERD.H" 

15  #INCLUDE  "UTILS. H" 

16  #INCLUDE  "MATH.H" 

17 

18 

19  UNSIGNED  INT  PIFLAG=0; 

20  UNSIGNED  INT  MAGIC=0X  1237; 
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21  EXTERN  CHAR  JOYSTICK^];  /*  DEFINED  IN  SHEPHERD.C  */ 

22  EXTERN  CHAR  BCDSTRINGf];  /*  DEFINED  IN  SHEPHERD.C  */ 

23 

24 

25  /* - - * 

26  *  READCLOCK()  * 

27  *  * 

28  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2 

* 

29  *  LAST  UPDATE:  26  FEBRUARY  1997  * 

30  *  NAME:  THORSTEN  LEONARD Y  * 

3 1  *  PURPOSE:  THIS  FUNCTION  READS  THE  VALUES  FROM  THE 
CALENDAR  CLOCK  * 

32  *  DEVICE  MTK48T08  (SEE  OMNIBYTE  HANDOUT  CHAP. 

2.9.4)  INTO  * 

33  *  GLOBAL  VARIABLE  CLOCK.  THE  FORMAT  IN  CLOCK 

IN  DECIMAL  IS:  * 

34  *  * 

35  *  CLOCK  =  YYMMDDHHMMSS  * 

36  *  * 

37  *  I.E  TO  RETRIEVE  THE  DATE  PERFORM 

DATE=CLOCK/ 1000000;  * 

38  *  TO  RETRIEVE  THE  TIME  PERFORM 

TIME=CLOCK%  1000000;  * 

39  *  * 


40  *  CALLED  BY: 


FUNCTION  TIMERHANDLER  IN  TIMER.C’ 


41 


42  * - */ 

43 

44 

45 

46 

47 

48  /* - * 

49  *  PITESTO  * 

50  *  * 

51  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2 

* 

52  *  LAST  UPDATE:  24  FEBRUARY  1997  * 

53  *  NAME:  THORSTEN  LEONARDY  * 

54  *  PURPOSE:  THIS  FUNCTION  TESTS  INTERPROCESSOR 

SIGANNLING  VIA  PI-46  * 

55  *  INTERRUPT.  * 

56  * - */ 

57  VOID  PITEST(VOID) 

58  { 

59  LONG  *VADR; 

60  UNSIGNED  CHAR  *P; 

61 

62  /*  SET  ADDRESS  FOR  PROCESSOR  INTERRUPT  HANDLER 

ROUTINE  */ 
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63  VADR=(UNSIGNED  LONG  *)VBA_PI; 

64  *VADR=(UNSIGNED  LONG)PIHANDLER; 

65 

66  P=(UNSIGNED  CHAR  *)ISM_PI; 

67  *P=(UN SIGNED  CHAR)0XE0;  /*  SET  IP-ISM  TO  68040  ON 
LIRQ-6  */ 

68 

69  P=(UNSIGNED  CHAR  *)VIC_LIRQ6; 

70  *P=(UNSIGNED  CHAR)0X06;  /*  CONFIGURE  VIC068  LIRQ-6 

*/ 

71 

72  P=(UNSIGNED  CHAR  *)APP_ICR;  /*  ABORT/PROC 

INTERRUPT  CTRL  */ 

73  *P  =  *P  I  (UNSIGNED  CHAR)0X02;  /*  ASSERT  EP-46  INTERRUPT 

*/ 

74 

75  WHTT  F/PTFT .  AG==0)  {  INTCOUNTER=0;  }  /*  WAIT  FOR  PI 
INTERRUPT  */ 

76 

77  IF  (PIFLAG==MAGIC)  { 

78  /*  TOGGLE VME( (UNSIGNED  CHAR  *)VME92 10,0X02);  */ 

79  SIOOUT(0, "PASSED"); 

80  } 

81  ELSE  { 

82  /*  TOGGLE VME( (UN S IGNED  CHAR  *)VME942 1 ,0X02);  *1 

83  SIOOUT(0, "FAILED"); 
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84  } 

85 

86  RETURN; 

87  }  /*  END  OF  PITEST  */ 

88 

89 

90 

91  /* _ * 

92  *  SETVME()  * 

93  *  * 

94  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2 

* 

95  *  LAST  UPDATE:  24  FEBRUARY  1997  * 

96  *  NAME:  THORSTEN  LEONARDY  * 

97  *  PURPOSE:  THIS  FUNCTION  OUTPUTS  DATA  TO  THE 

STATUS  REGISTER  OF  THE  * 

98  *  SPECIFIED  VME  BOARD.  * 

99  * - */ 

100  VOID  SET VME(UN S IGNED  CHAR  *BOARD  ADDRESS,  UNSIGNED 
CHAR  DATA) 

101  { 

102  BOARD  ADDRESS  =  BOARD  ADDRESS  +  0X81;  /*  ACCESS 

STATUS  REGISTER  *1 

103  *BOARDADDRESS=DATA;  /*  WRITE  DATA  */ 

104  RETURN; 

105  } 
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106 


107 

108 

109  /* - * 

110  *  TOGGLEVME()  * 

111  *  * 

112  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2 

* 

113  *  LAST  UPDATE:  24  FEBRUARY  1997  * 

114  *  NAME:  THORSTEN  LEONARDY  * 

115  *  PURPOSE:  THIS  FUNCTION  PERFORMS  AN  XOR 

OPERATION  ON  THE  STATUS  * 

116  *  REGISTER  OF  THE  SPECIFIED  VME  BOARD. 


117  * - */ 

118  VOID  TOGGLEVME(UNSIGNED  CHAR  *BOARD,  UNSIGNED 
CHAR  DATA) 

119  { 

120  BOARD  =  BOARD  +  0X8 1 ;  /*  ACCESS  STATUS  REGISTER  */ 

121  *BOARD  =  *BOARD  A  DATA;  /*  TOGGLE  BIT  WITH  BITWISE 
XOR*/ 

122  RETURN; 

123  } 

124 

125 

126 
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127  /* 


128  *  INITBOARDSO 

129  * 

130  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2 

131  *  LAST  UPDATE:  24  FEBRUARY  1997 


132  *  NAME:  THORSTEN  LEONARDY  * 

133  *  PURPOSE:  THIS  FUNCTION  INITIALIZES  ALL  VME  BOARDS. 

* 

134  * - */ 


135  VOID  INITBO ARDS  (VOID) 

136  { 

1 37  UNSIGNED  CHAR  *P; 

138  INT IX; 

139 

140  P=(UNSIGNED  CHAR*)VIC_TTR;  /*  VIC  TRANSFER 

TIMEOUT  REGISTER  */ 

141  *P  =  OXFF;  /*  DISBLE  ALL  WATCHDOGS  */ 

142 

143  P=(UNSIGNED  CHAR*)VIC_ICR;  /*  VIC  INTERFACE 

CONFIGURATION  REG.  */ 

144  *P=0X40;  /*  PREVENT  DEADLOCKS,  THIS  IS  A 

MUST!  */ 

145 

146  P=(UNSIGNED  CHAR*)VME942 1+0X81;  I*  ACCESS  STATUS 
REGISTER  FOR  DI  */ 
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/*  DISABLE  SYSFAIL  SIGNAL,  SET 


147  *P  =  0X03; 

GREEN  */ 

148 

149  P=(UNSIGNED  CHAR* )VME92 10+0X81;  /*  ACCESS  STATUS 
REGISTER  FOR  DA  */ 

150  *P  =  0X03;  /*  DISABLE  SYSFAIL  SIGNAL,  SET 

GREEN  */ 

151 

152 

153  /* - * 

154  *  INITIALIZE  ALT.  EIGHT  QUADRATURE  COUNTERS  (WHEEL 
ENCODER)  * 

155  * - */ 

156 

1 57  P=(UNSIGNED  CHAR*)VMECTR1 ; 

158  FOR  (EX=0;  IX<8;  EX++)  {  /*  READ  MOTORS 

SUBSEQUENTIALLY  */ 


159 

*(P+3)=0X20; 

/*  CR:  MASTER  RESET 

*/ 

160 

*(P+3)=0X48; 

/*  IC:  ENABLE  COUNTING 

*/ 

161 

*(P+3)=0XC1; 

/*  QR:  COUNT  FULL  CYCLE 

*/ 

162 

P=P+0X04; 

/*  ACCESS  NEXT  COUNTER 

*/ 

163 

IF  (IX==3)  P= 

=(UNSIGNED  CHAR  * )  VMECTR2 ;  /*  ACCESS  THE 

SECOND  VME  COUNTER  */ 

164  ) 

165 

166  SIOOUT(0,"BOARDS  INITIALIZED  ...\N\RM); 
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167 


168  RETURN; 

169  } 

170 

171  /*  MODIFIED  ED  MAYS  1 8  APR  97  */ 

172  UNSIGNED  CHAR  B2A(INT  VALUE) 

173  { 

174  UNSIGNED  CHAR  CHAR; 

175 

176  IF  (VALUE  <  10)  { 

177  CHAR  =  48  + VALUE; 

178  } 

179  ELSE  { 

180  CHAR  =  55  + VALUE; 

181  } 

182  RETURN  CHAR; 

183  } 

184 

1 85  VOID  B2A2(UNSIGNED  CHAR  *S,  UNSIGNED  CHAR  CC) 

186  { 

187  INT  LOW,  HIGH; 

188 

189  LOW  =  CC  &  0X0F; 

190  HIGH  =  CC/16; 

193 


191 


192  *S  =  B2A(LOW); 

193  *(S-1)  =  B2A(HIGH); 

194  } 

195 

196 


197 

I* **************************************************************** 


198  ASSEMBLER  ROUTINES 

199 

* ******************************************************************* * 
*******/ 

200 

201  ASM(" 

202  .EVEN 

203  .TEXT 

204  .GLOBL  _PIHANDLER 

205 

206  _PIHANDLER: 

207  MOVE.L  #0XFFF4800C,A  1  /*  LOAD  APP-ICR  INTO  A1 

*/ 

208  AND.B  #0XFD,(A1)  /*  REMOVE  PENDING  IP-46 

INTERRUPT  SIGNAL  */ 

209  MOVE.L  #0X  1 237,_PIFLAG  /*  SET  PIFLAG  VARIABLE 

*/ 

210  RTE 
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211 


); 

212 

213 

214  /* - * 

215  *  CONVERTTOASCIIO  * 

216  *  * 

217  *  ENVIRONMENT:  GCC  COMPILER  V2. 7. 2  * 

218  *  LAST  UPDATE:  02  MAY  1997  * 

219  *  NAME:  THORSTEN  LEONARDY  * 

220  *  PURPOSE:  THIS  FUNCTION  CONVERTS  AN  UNSIGNED 

INTEGER  TO  ITS  ASCII  * 

221  *  EQUIVALENT  AND  WRITES  THIS  INTO  A  STRING. 

* 

222  *  NDIGITS  NUMBER  OF  DIGITS  TO  CONVERT 

* 

223  *  DATA  THE  INTEGER  TO  CONVERT  * 

224  *  STR  POINTER  TO  STRING  * 

225  * - */ 

226  VOID  CON VERTT O ASCII(UNS IGNED  INT  NDIGITS,  UNSIGNED 
INT  DATA,  CHAR  *STR) 

227  { 

228  UNSIGNED  INT  I; 

229 

230  STR=STR+NDIGITS- 1 ; 

231  FOR  (I=0;I<NDIGITS;I++)  { 
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232  *STR~  =  ’0’  +  DATA  %  10  ; 

233  DAT  A=D  AT  A/ 1 0; 

234  } 

235  RETURN; 

236  } 

237 

238 

239  /* - * 

240  *  READJOYSTICK0  * 

241  *  * 

242  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2 

* 

243  *  LAST  UPDATE:  02  MAY  1997  * 

244  *  NAME:  THORSTEN  LEONARDY  * 

245  *  PURPOSE:  THIS  FUNCTION  READS  THE  THREE  PORTS  (A,B 

AND  C)  FROM  THE  * 

246  *  INTEL  85C55  PARALLEL  PORT  1  AND  CONVERTS 

THEM  INTO  AN  ASCII  * 

247  *  STRING.  * 

248  * - */ 

249 

250  VOID  READJOYSTICK(VOID) 

251  { 

252  UNSIGNED  INT  I,INDEX; 

253  UNSIGNED  CHAR  *CTRLPORT=(UNSIGNED  CHAR*)PIO  \  _CTRL; 
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254  UNSIGNED  CHAR  *D  AT  APORT =(UN  S IGNED 

CHAR*)PI01_DATA; 

255  UNSIGNED  INT  PIOPORT1  [3]; 

256  DOUBLE  A=  0. 1 ,  XX,  YY,  ZZ; 

257 

258  *CTRLPORT=OX9B ;  /*  SET  ALL  PORTS  (A,B,C)  INTO  INPUT 
MODE  (READ  ONLY)  */ 

259  INDEX=10;  /*  POSITION  FOR  X-DIGITS  IN  STRING 

JOYSTICK  */ 

260 

261  FOR  (I=0;I<3;I++) 

262  PIOPORT 1  [I]  =  *  (DAT  APORT +1) ; 

263 

264  XX  =  (DOUBLE)PIOPORTl  [0]- 128.0; 

265  YY  =  (DOUBLE)PIOPORT  1  [  1  ]- 1 28 .0; 

266  IF  (XX  >=  0.0) 

267  XX  =  XX*XX/100; 

268  ELSE 

269  XX  =  -XX*XX/100; 

270  IF  (YY  >=  0.0) 

271  YY  =  YY*YY/100; 

272  ELSE 

273  YY  =  -YY*YY/100; 

274  JOYSTICKS  =  A*(XX)  +  ( 1 ,0-A)*JOYSTICK.X; 

275  JOYSTICK.  Y  =  A*(YY)  +  ( 1 .0-A)*JOYSTICK.Y; 
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276 


277  IF  (PIOPORT 1  [2]==0X03) 

278  SETVME((UNSIGNED  CHAR  *)VME92 10,0X00);  /*  NO  BUTTON 
PRESSED  */ 

279  ELSE  { 

280  SETVME((UNSIGNED  CHAR  *)VME92 10,0X02);  /*  IF  ANY 
BUTTON  PRESSED  */ 

281  } 

282  } 

283 

284 

285  DOUBLE  INSENS ITIVE(DOUBLE  Z) 

286  { 

287  IF  (Z  >=  10.0) 

288  RETURN  (Z  - 10.0); 

289  ELSE 

290  IF  (Z  <=  -10.0) 

291  RETURN  (Z+  10.0); 

292  ELSE 

293  RETURN  0.0; 

294  ) 

295 

296  VOID  DISPLAYJOYSTICK0 

297  { 

298  CONVERTINT(BCDSTRING+9,  (INT)JOYSTICK.X); 
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299  BCDSTRING[3]='0'; 

300  BCDSTRING[4]=’3'; 

301  BCDSTRING[6]='4'; 

302  BCDSTRING[7]='0'; 

303  SIOOUT(0,BCDSTRING); 

304 

305  CONVERTINT(BCDSTRING+9,  (INT)JOYSTICK.OMEGA); 

306  BCDSTRING[3]='0'; 

307  BCDSTRING[4]='4'; 

308  BCDSTRING[6]='4'; 

309  BCDSTRING[7]='0'; 

310  SIOOUT(0,BCDSTRING);  /*  OUTPUT  UPDATED  POSITION 
STRING  TO  SCREEN  */ 

311  RETURN; 

312  } 

313 

314 

315  END  OF  UTILS. C 

316 

********************************************************************* 

*******f 
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200 


APPENDIX  Is  SOURCE  CODE  (SERIAL.C) 


1  /* - * 

2  *  * 

3  *  FILE:  SERIAL.C  * 

4  *  * 


5  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

6  *  LAST  UPDATE:  26  FEBRUARY  1997  * 

7  *  NAME:  THORSTEN  LEONARDY  * 

8  *  PURPOSE:  PROVIDES  ROUTINES  FOR  SERIAL  INPUT 

AND  OUTPUT  TO  THE  68C681  * 

9  *  ON  THE  TAURUS  BOARD.  * 

10  *  * 

1 1  *  COMPILED:  >GCC  -C  -M68040  -O  SERIAL.O  SERIAL.C* 

12  *  * 

13  * - */ 

14 

15  #INCLUDE  "SHEPHERD.H" 

16  #INCLUDE  "SERIAL.H" 

17 

18  /* - * 

19  *  GLOBAL  VARIABLES  * 

20  * - */ 

21 
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22  /*  UNSIGNED  INT  COUNTER;  /*  COUNT  THE  INTERRUPTS  */ 

23  UNSIGNED  CHAR  INPORTA;  /*  CHARACTER  READ  FROM  SERIAL 
PORT  */ 

24 

25  /*  VT1 00  CONTROL  SEQUENCES  */ 

26 

27  /*  POSITION  CURSOR,  CUP  =  ESC  [  'O'  ’O' ;  'O'  '0'  H  */ 

28  UNSIGNED  CHAR  VT100XY[9]={27, 91, 48, 48,59, 48, 48,72,0};  /*  POSITION 

CURSOR  */ 

29 

30  /*  ERASE  IN  DISPLAY  ED  TO  CLEAR  THE  SCREEN  */ 

31  UNSIGNED  CHAR  CLRSCR[5]= {27,91,50,74,0};  /*  ESC  [  '2' J  */ 

32 

33  /*  ESC-SEQUENCE  EL  (ERASE  IN  LINE)  TO  ERASE  A  LINE  */ 

34  UNSIGNED  CHAR  CLRLINE[6]=  {5,27,91,50,75,0};  /*  ESC  [  '2'  K  */ 

35 

36  /*  ESC-SEQUENCE  PRINT  SCREEN  (ESC  [  I)  */ 

37  UNSIGNED  CHAR  PRTSCR[4]=  {27,91 ,1 05,0} ;  /*  ESC  [  I  */ 

38 

39 

40  /*  ESC-SEQUENCE  SGR  (SELECT  GRAFIK  RENDITION)  (ESC  [  0  M )  */ 

41  UNSIGNED  CHAR  CURSOROFF[5]=  {27,91,0,109,0};  /*  CURSOR  BUNK 

OFF*/ 

42 

43  /* - * 
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44  *  SIOOUTO  51 

45  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

46  *  LAST  UPDATE:  07  JANUARY  1997  * 

47  *  NAME:  THORSTEN  LEONARDY  * 

48  *  PURPOSE:  THIS  FUNCTION  OUTPUTS  A  STRING 

TO  ONE  OF  THE  TWO  SERIAL  * 

49  *  PORTS.  51 

50  *  * 

51  *  HOSTFLAG  0  ->  OUTPUTS  TO  CONSOLE  (PORT  A)  * 


52  *  1  ->  OUTPUTS  TO  HOST  (PORT  B)  * 

53  *  * 

54  *S  POINTER  TO  THE  OUTPUT  STRING  * 

55  *  * 

56  * - */ 

57 


58  VOID  SIOOUT(INT  HOSTFLAG,  UNSIGNED  CHAR  *S) 

59  { 

60  UNSIGNED  CHAR  *P= (UNSIGNED  CHAR  *)CONSOLE; 

61 

62  IF  (HOSTFLAG)  P+=8;  /*  ACCESS  HOST  REGISTERS  */ 

63  /*  OTHERWISE  ACCESS  CONSOLE  */ 

64  WHILE(*S)  { 

65  WHILE  ((*(P+1)&4)==0);  /*  SRA:  WAIT  UNTIL  TX  READY  */ 

66  *(P+3)=*S++;  /*  OUTPUT  CHARACTER  */ 
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67 


} 


68 

69  RETURN; 

70  }  /*  END  OF  SIOOUT  */ 

71 

72 

73 

74  /* - * 

75  *  GOTOXYO  * 

76  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

77  *  LAST  UPDATE:  14  FEBRUARY  1997  * 

78  *  NAME:  THORSTEN  LEONARDY  * 

79  *  PURPOSE:  THIS  FUNCTION  POSITIONS  THE  CURSOR  ON  THE 

SCREEN.  * 

80  *  * 

81  *X  ROW  FOR  CURSOR  POSITION  (X=0..20)  * 

82  *  Y  COLUMN  FOR  CURSOR  POSITION  (Y=l.. 80)  * 

83  *  * 

84  * - */ 

85 

86  VOID  GOTOXY (INT  X,  INT  Y) 

87  { 

88 

89  IF  ((X>0)&(X<81)&(Y>0)&(Y<33))  { 
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90 


VT1 00XY[2]  =48 +X/ 10; 

91  VT1 00XY[3] =48+X%l  0; 

92  VT1 00XY[5]  =48 +Y  /1 0; 

93  VT1 00XY[6]  =48+Y%l  0; 

94  SIOOUT(0,VT100XY);  /*  OUTPUT  ESCAPE  -SEQUENCE  */ 

95  } 

96  RETURN; 

97  } 

98 

99 

100  /* - * 

101  *  SIOINITO  * 

102  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

103  *  LAST  UPDATE:  26  FEBRUARY  1997  * 

104  *  NAME:  THORSTEN  LEONARDY  * 

105  *  PURPOSE:  THIS  FUNCTION  INITIALIZES  BOTH 

SERIAL  PORTS.  IN  ADDITION,  * 

106  *  PORT  A  (CONSLE)IS  INITIALIZED  FOR 

INTERRUPT  DRIVEN  I/O  * 

107  * - */ 

108 

109  VOID  SIOINIT(VOID) 

110  { 
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1 1 1  UNSIGNED  CHAR  *P= (UNSIGNED  CHAR*)  CONSOLE;/*  BASE  ADDRESS 
FOR  68C681  DUART  */ 

112  LON G  * V ADR;  /*  FOR  VBA  REGISTER  ENTRY  */ 


113 

114 

115  /* - */ 

116  /*  INITIALIZE  CONSOLE  (PORT  A)  */ 

117  /* - */ 

118  /*  ATTENTION:  THESE  SETTINGS  HAVE  TO  AGREE  WITH  THE 
SETTINGS  FOR  */ 

119  /*  YOUR  TERMINAL  (LE.  LAPTOP  COMPUTER)  */ 


120  *(P+2)= (UNSIGNED  CHAR)0X2A;  /*  CRA:  RESET  RX,DISABLE  RX  & 
TX  */ 

121  *(P+2)=(UNSIGNED  CHAR)0X1A;  /*  CRA:  RESET  MR  POINTER, 

*/ 

122  *(P+0)=(UNSIGNED  CHAR)0X13;  /*  MR1A:  RX  CONTROLS  RTS, 

V 

123  /*  8  BITS,  NO  PARITY  */ 

124  *(P+0)= (UNSIGNED  CHAR)0X07;  /*  MR2A:  NORMAL  MODE,  1  STOP 
BIT  */ 

125  *(P+1)=(UNSIGNED  CHAR)0XBB;  /*  SET  BAUD  RATE  9600  BAUD 

*/ 

126  *(P+2)=(UNSIGNED  CHAR)0X15;  /*  ENABLE  RX  AND  TX 

*/ 

127 

128 

129  /* - */ 

1 30  /*  INITIALIZE  HOST  (PORT  B)  */ 
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131 

/* 

- */ 

V 

132 

*(P+10)=(UNSIGNED  CHAR)0X1A; 

/*  CRB:  RESET  MR  POINTER 

V 

133 

*  (P+ 8) = (UN SIGNED  CHAR)0X13; 

/*  MR1B:  NO  PARITY,  8  BITS 

BIT 

134 

V 

*(P+8)=(UNSIGNED  CHAR)0X07; 

/*  MR2B:  NORMAL  MODE,  1  STOP 

V 

135 

*(P+9)=(UNSIGNED  CFIAR)0XBB; 

/*  SET  BAUD  RATE  9600  BAUD 

V 

136 

*(P+ 10)= (UNSIGNED  CHAR)0X15; 

/*  CRB:  ENABLE  RX  AND  TX 

137 

138 

139 

/* 

- v 

V 

140 

/*  IT  FOLLOWS  THE  INTERRUPT  SPECIFIC  PART  FOR  PORT  A 

141 

/* - 

- */ 

142 

*(P+5)=(UNSIGNED  CHAR)0X02; 

/*  ISR:  SET  INTERRUPT  MASK  FOR 

RXRDY  A  */ 

143  *(P+12)=0X60;  /*  IVR:  PLACE  INTERRUPT  VECTOR  */ 

144  /*  0X60  ACCESSES  VBA  AT  BASE+0X1 80  */ 

145  VADR=(LONG*)0XFFE401 80;  /*  VBA  ADDRESS  FOR  INTHANDLER 

V 

146  *VADR=(LONG)INPORTAHANDLER;  /*  WRITE  ADDRESS  INTO  VBR 

*/ 

147 

148  P= (UNSIGNED  CHAR*)ISM_SERIAL;  /*  ISM  CONFIGURATION 
REGISTER  FOR  SIO  */ 

149  *P=0X09;  /*  INTERRUPTS  TO  68040  ON  LIRQ-1  */ 
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150 


151 

ISM  */ 


P= (UNSIGNED  CHAR*)VIC_LIRQ1;  /*  VIC068  OCR  FOR  LIRQ-1  FROM 


/*  ASSERT  IRQ-1  FROM  VIC  TO  68040  */ 


152  *P=0X01; 

153 

154 

155  RETURN; 

156  } 

157 

158 

159 

160 

161  /**************************************************************************** 

162  ASSEMBLER  ROUTINES 

1  63  ****************************************************************************  / 

164 

165  /* - * 

166  *  INPORTAHANDLERO  * 

167  *  * 

168  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

169  *  LAST  UPDATE:  27  JANUARY  1997  * 

170  *  NAME:  THORSTEN  LEONARDY  * 


171  *  PURPOSE: 

FROM  68C681  DU  ART  * 


INTERRUPT  HANDLING  ROUTINE  FOR  INTERRUPTS 


208 


172*  IT  READS  A  CHARACTER  INPUT  FROM  THE  KEYBOARD  INTO 

VARIABLE  * 

173  *  INPORTA,  INCREMENTS  A  COUNTER,  AND  OUTPUTS  THE 

CHARACTER  * 

174  *  TO  THE  SCREEN.  IF  A  CR  IS  TYPED  AT  THE  KEYBOARD,  AN 

* 

175  *  ADDITIONAL  LINEFEED  (OXOA)  IS  ADDED  TO  THE  <CR> 

(OXOD).  * 

176  *  * 

177  * - */ 


178 

179  ASM(" 

180 

181  .EVEN 

182  .TEXT 

1 83  .GLOBL  _INPORTAHANDLER 

184 

185  _INPORTAHANDLER: 

186 

187  LINK  A6,#-128  /*  ALLOCATE  184  BYTES  ON  STACK  TO  ...  */ 

188  FSAVE  A6@(-128) 

189  MOVEML  D0-D7/A0-A5,SP@-  /*  SAVE  REGISTERS  (14*4  BYTE) 

190 

191  MOVE.L  #0XFFF4A000A2  /*  BASE  ADDRESS  OF  68C681  DU  ART 

*/ 

192  MOVE.B  3(A2),D2  /*  RHR_A:  READ  CHARACTER  ...  */ 
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193 


MOVE.B  D2,_INPORTA 


/*  ...  AND  COPY  TO  INPORTA 


V 


194 

195 

196 

197 

198 

199 

200 
201 
202 

203 

204 

205 

206 
207 


MOVEML  SP@+,D0-D7/A0-A5 
FRESTORE  A6@(-128) 

UNLK  A6 


RTE 


END  OF  SERIAL.  C 
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APPENDIX  J:  SOURCE  CODE  (CONSOLIDATED  HEADER  FILES) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /*  shepherd.h  */ 

2 

3  #ifndef  SHEPHERD.H 

4  #define  SHEPHERD.H 

5 

6  /* - * 

7  *  Base  Addressees  for  accessing  Servo  Control  Cards  * 

8  * - */ 

9 

10  #define  VME9210  0xffff0400  /*  Base  Address  analog  out  to  servo  */ 

1 1  #define  VME9421  OxffffOOOO  /*  Base  address  data  in  from  servo  */ 

12  #define  VME2170  OxffffffOO  /*  Base  address  data  out  to  servo  */ 

13  #define  VMECTR1  0xffff6000  /*  VME  Counter  for  driving  motor  */ 

14  #define  VMECTR2  0xffff6100  /*  VME  Counter  for  steering  motor  */ 

15 

16 

17  /* - * 

18  *  defines  for  general  Interrupt  Handling  * 
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19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 


* 


-  */ 


#define  VIC_LIRQ1  0xfff44027 
#define  VIC_LIRQ2  0xfff4402b 
#define  VIC_LIRQ3  0xfff4402f 
#define  VIC_LIRQ4  0xfff44023 
#define  VIC_LIRQ5  0xfff44037 
#define  VIC_LIRQ6  0xfff4403b 
#define  VIC_LIRQ7  0xfff4403f 

#define  VIC_TTR  0xfff44043 

#define  VIC_ICR  0xfff440af 


/*  VIC068  Register  for  LIRQ-1  */ 
/*  VIC068  Register  for  LIRQ-2  *1 
/*  VIC068  Register  for  LIRQ-3  */ 
/*  VIC068  Register  for  LIRQ-4  */ 
/*  VIC068  Register  for  LIRQ-5  */ 
/*  VIC068  Register  for  LIRQ-6  */ 
/*  VIC068  Register  for  LIRQ-7  */ 


/*  Transfer  Timeout  Register  */ 
/*  see  p.  4-2  TAURUS  Manual  */ 

/*  VIC  Interface  Configuration  */ 


#define  enable()  asm("move.w  #0x2000,sr")  /*  enable  interrupts  */ 
#define  disable!)  asm("move.w  #0x2700,sr")  /*  disable  interrupts  */ 


/*  defines  for  Vector  base  register  entries  */ 

#define  VBA_TIMER  0xffe40 1 30  /*  Vector  table  address  for  Timer-5  ISR  */ 

#define  VBA_PI  0xffe401 1 8  /*  Vector  table  entry  for  EP  interrupt  */ 
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41 


42 

43  /*  — - * 

44  *  defines  for  interrupt  steering  mechanism  * 

45  * - */ 

46 

47  #define  ISM_TIMER  0xfff48004  /*  ISM  Configuration  Register  for  Timer  A  */ 

48  #define  ISM_PI  0xfff48008  /*  ISM  Configuration  Register  for  PI  */ 

49  #define  ISM.SERIAL  0xfff48001  /*  ISM  Configuration  Register  for  serial  10  */ 

50 

5 1  #define  APP_ICR  0xfff4800c  /*  abort/processor  interrupt  control  register  */ 

52 

53 

54 

55  /* - * 

56  *  Base  Addressees  for  accessing  Parallel  IO-Ports  * 

57  * - */ 

58  #define  PIO 1  _CTRL  0xfff40003  /*  control  register  for  PIO- 1  */ 

59  #define  PIO  1  _DATA  0xfff40000  /*  data  register  for  PIO- 1  Port  A  */ 

60  #define  PI02_CTRL  0xfff40007  /*  control  register  for  PIO-2  */ 

61  #define  PI02_DATA  0xfff40004  /*  data  register  for  PIO-2  Port  A  */ 

62 
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63 


64  /* - * 

65  *  Base  Addressees  for  68030  Input/Output  Program  * 

66  *  as  outlined  in  Taurus  Manual,  Chapter  6  * 

67  * - */ 

68  #define  IOP_CMDBLK  OxffeOOOOO  I*  address  for  IOP  Command  Block  */ 

69  #define  IOP_START  0x01  /*  command  to  start  IOP  */ 

70  #define  IOP_STOP  0x00  /*  command  to  stop  IOP  */ 

71  #define  IOP.COMPLETE  0x80  /*  mask  for  operation  complete  */ 

72 

73  #define  IOPB_CONFIGURE  OxeO  /*  command  to  configure  IOBP  */ 

74  #define  IOPB_UNIT_OMNIO  0x10  /*  unit  #  for  omnimodule  #0  */ 

75  /* - * 

76  *  definitions  for  68030  Input/Output  Program,  (Leo,  05/13/97)  * 

77  * - */ 

78 

79 

80  /*  Input/Output  Parameter  Block  structure,  according  Taurus  Manual,  p.  6-4  */ 

8 1  typedef  struct  { 

82  unsigned  char  cmd;  /*  command  */ 

83  unsigned  char  error;  /*  error  status  */ 

84  unsigned  short  options;  /*  options  */ 
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85  unsigned  short  reserved;  /*  reserved,  do  not  use  */ 

86  unsigned  char  unit;  /*  unit  number  */ 

87  unsigned  char  destUnit;  /*  destination  unit  */ 

88  unsigned  long  blockNumber;/*  logical  Block  number  */ 

89  unsigned  long  txCount;  /*  Transfer  count,  #  of  bytes  to  transfer  */ 

90  unsigned  long  *ptrSrc;  /*  address  of  source  */ 

91  unsigned  long  *ptrDst;  /*  Address  of  destination  */ 

92  }IOPB; 

93 

94  /*  Command  Block  structure  according  to  Taurus  Manual,  p.  6-3  */ 

95  typedef  struct  { 

96  unsigned  char  cmd;  /*  status  and  command  register  */ 

97  unsigned  char  reserved[3];  /*  not  yet  used  */ 

98  IOPB  *ptrToIOPB;  /*  pointer  to  IOBP  */ 

99  }CMD_BLOCK; 

100 
101 

102/*  Omnimodule  support  block  structure  according  to  Taurus  Manual  p.  6-12  */ 

103  typedef  struct  { 

104  unsigned  long  options;  /*  4  bytes  options,  unused  */ 

105  unsigned  long  *ptrlnit;  /*  pointer  to  initialization  routine  */ 

106  unsigned  long  *ptrTask;  /*  pointer  to  task  */ 
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107  unsigned  long  *ptrlntr;  /*  pointer  to  interrupt  servicing  routine  */ 

108JOSB; 

109 

1 10IOPB  iopbOMNIO;  /*  IOBP  for  Omnimodule  0  (used  for  serial  I/O  to  VT100  */ 
lllOSB  osbOMNIO;  /*  OSB  for  Omnimodule  0  (used  for  serial  I/O  to  VT100  */ 


112 

113/* - */ 

1 14/* - */ 

115 

1 1 6unsigned  int  intCounter,  testCounter;  /*  count  the  interrupts  */ 


1 17unsigned  int  demo;  /*  switch  to  run  demo  see  driver()  in  movement.c  */ 
1 1 8unsigned  short  timer_in_ms;  /*  desired  timer  period  in  ms  */ 

119 

120 

121/* - * 

122  *  definitions  for  inertial  measurement  routines  (imu.c)  * 

123  * - */ 

124 

125/*  added  10  Sep  97  */ 

1 26typedef  struct  { 

127  unsigned  short  ax;  /*  linear  acceleration  in  x-direction  */ 

128  unsigned  short  ay;  /*  linear  acceleration  in  y-direction  */ 
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129  unsigned  short  az;  /*  linear  acceleration  in  z-direction  */ 

130  unsigned  short  omega_z;  /*  angular  velocity  in  z-direction  */ 


131  }IMU; 

132 

133IMU  imu;  /*  stores  most  recent  IMU  data  (updated  with  */ 

134  /*  every  10ms  timer  interrupt  */ 

135 

136/* - * 


137  *  definitions  for  Joystick  Control,  (Leo,  05/10/97) 


138  * - */ 

139 

140typedef  struct  { 

141  double  x;  /*  x  position  (or  velocity)  */ 

142  double  y;  /*  y  position  (or  velocity)  */ 

143  double  omega;  /*  angular  velocity  */ 


144  unsigned  char  state;  /*  status  of  parallel  port  1,  channel  C  */ 
145JJPOINT; 

146  JPOINT  joystick;  /*global*/ 

147 

148typedef  struct  { 

149  double  x; 

1 50  double  y; 
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151  }point; 

152 

153 

154typedef  struct  { 

155  point  coord; 

156  double  heading; 

157  double  kappa; 

158}  Configuration; 

159  Configuration  vehicle;  /*global*/ 

160 

161typedef  struct  { 

162  double  Speed; 

163  double  Theta; 

164  double  Omega; 

1 65 }  vehicleMotion; 

166  vehicleMotion  motion, motionO;  /*global*/ 

167 

168typedef  struct  { 

1 69  double  rho; 

170  double  alpha; 

171 }  polar; 

172 
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173 


174/* - */ 

175/*  definitions  for  wheel  control  */ 

176/* - */ 

177 

178/*  write  these  masks  to  VME2710  at  address  OxffffffOO  in  order  to  make  */ 
179/*  the  specific  motor  drive!  May  wish  to  logical  OR  with  previous  settings  */ 
180 


181  #define  TURN_FR 

0x00004000 

182#define  TURN.FL 

0x00020000 

183#define  TURN.RR 

0x00100000 

184#define  TURN.RL 

0x00800000 

185 

1 86#define  DRIVE_FR 

0x00000004 

1 87#define  DRIVE_FL 

0x00000020 

188#define  DRIVE_RR 

0x00000100 

1 89#define  DRIVE_RL 

0x00000800 

190 

/*  turn  wheel  1  (front  right)  */ 
/*  turn  wheel  2  (front  left)  */ 

/*  turn  wheel  3  (rear  right)  */ 
/*  turn  wheel  4  (rear  left)  */ 

/*  drive  wheel  1  (front  right)  */ 
/*  drive  wheel  2  (front  left)  */ 
/*  drive  wheel  3  (rear  right)  */ 
/*  drive  wheel  4  (rear  left)  */ 


1 9 1  #define  ALL_WHEELS  0x00924924  /*  select  all  wheels  for  turning  */ 


192  /*  and  driving  */ 

193 

194/* - * 
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195  *  function  definitions  * 


196  * - */ 

197 

198 void  setVME(unsigned  char  *board,  unsigned  char  data); 

199 void  toggle VME(unsigned  char  *board,  unsigned  char  data); 
200void  initBoards(void); 

201  void  piTest(void); 

202void  piHandler(void); 

203 void  advanceCount(); 

204  /*  global  variable  to  make  joystock  coordinates  accessible  */ 

205 

206#define  ARRAY.SIZE  4 

207#define  DegreesToRads  0.0174532925 

208#define  RadsToDegrees  57.29577951308232 

209#define  DeltaT  0.01 

210 

211 

2 12double  desiredAngleRates[ARRAY_SIZE], 

213  desiredAngleRatesO[ARRAY_SIZE], 

214  desiredSpeeds_F[ARRAY_SIZE], 

2 1 5  desi  red  AngleRates_F[  ARRA  Y_SIZE] , 

216  desiredSpeeds[ARRAY_SIZE], 
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217  actualSpeeds [  ARR  A  Y_SIZE] ,  /*  28  May  ejm  */ 

218  actualAngleRates[ARRAY_SIZE], 

219  DigitToCmDrive[ARRAY_SIZE], 

220  Display_Speeds[ARRAY_SIZE], 

221  Display_Steers[ARRAY_SIZE], 

222 

223  desired  Angles  [  ARRA  Y_SIZE] , 

224  desiredAnglesO[  ARRA  Y_SIZE] , 

225  actual  Angles  [  ARR  A  Y_SIZE]; 

226 

227short  Steer_Digits[ARRAY_SIZE], 

228  Speed_Digits[ARRAY_SIZE] ; 

229 

230double  WheelDri  veAct[  ARRA  Y_SIZE] , 

23 1  WheelDriveDes[ARRAY_SIZE] ; 

232 

233unsigned  long  int  WheelDri veActO[ ARRA Y_SIZE], 

234  WheelDriveActl  [ARRAY_SIZE], 

235  driveReadings[  ARRA  Y_SIZE] ; 

236 

237double  WheelDirAct  [  ARRA  Y_S  IZE] , 

238  WheelDirDes[  ARRA  Y_S  IZE], 
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239 


PreviousCountSpeed[ARRAY_SIZE] , 
PreviousCountSteer[ARRA  Y_SIZE] ; 


240 

241 

242unsigned  long  int  WheelDirActO[ARRAY_SIZE], 

243  WheelDirAct  1  [  ARRAY_SIZE] , 

244  steerReadings[ARRAY_SIZE]; 

245 

246int  mode, 

247  oldMode, 

248  modeOstate, 

249  mode5state, 

250  modeTstate, 

251  Flag, 

252  oldFlag, 

253  edCounter, 

254  hallSensor3; 

255 unsigned  int  intCounter,  testCounter;  /*  count  the  interrupts  */ 

256 

257 

258 

259/*unsigned  long  int  */ 

260double  previousCount,previousCountSteer,  Omega_Speed, 
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261previousCountSpeed;/*previousCount  represents  infinity  */ 

262 

263 double  K 1  [ARRA  Y_SIZE] , 

264  K2[ARRAY_SIZE], 

265  K3  [  ARRAY_SIZE] , 

266  K4[ARRA  Y_SIZE] , 

267  K6[ARRAY_SIZE];  /*  slope  based  on  input  units  vs  output  velocity,  */ 

268  /*  input  range  from  0-  1020,  feedback  constant  */ 

269  /*  K3  is  the  inverse  of  (86.975velocity/1020  digit)  */ 

270 

271 

272#endif 

273 


275  End  of  shepherd.h 

277 

278 

279  #ifndef _ MOVEMENT_H _ 

280#define _ MOVEMENT_H _ 

281 

282#include  "shepherd.h" 
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285#define  PI 


3.14159265358979323846 


286#define  DPI  6.283 1 8530717958647692  /*  PI*2 

287#define  HPI  1.570796327  /*  PI/2  */ 

288#define  QPI  0.785398163  /*  PI/4  */ 

289#define  QPIby500  0.0015707963 

290  /*  QPI/(5  seconds/deltaT)  */ 

291 

292 

293 

294 

295double  wheel_speed[4],  wheel_angle[4]; 

296 

297 void  initMovement(); 

298void  setupPolar(polar  []); 

299void  wheelMotion(); 

300void  bodyMotion(); 

301  void  driver(); 

302void  joystickMotionlnterface(void); 


305 extern  double  desiredAngleRates[], 

306  desiredSpeeds[], 

307  PreviousCountSpeed  [] , 

308  PreviousCountSpeed[], 

309  PreviousCountSteer[]; 

310 

311polar  whp[4]; 

3 12double  pathLength,thetaDot,omegaDot,speedDot; 

313 

314 

315 

316/*  *************Jtenis  for  tangential  motion  **************  */ 

317 

318 

319double  sigma; 

320double  radius; 

321 

322double  ai[4],  bi[4]; 

323typedef  struct  { 

324  Configuration  config; 

325  point  center; 

326  double  radius; 
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327  double  a; 

328  double  b; 

329  double  c; 

330}  LINE; 

331 

332static  LINE  currentPath;  /*  holds  the  current  path  element  values  */ 
333Configuration  incrementalMotion,  holdVehicle; 

334static  double  deltaS; 

335 

336void  tangentialMotion(); 

337 void  circularArc(double  length,  double  alpha); 

338void  defineConfig(double  x, double  y,double  theta, double  kappa); 
339void  compose(); 

340double  steer(); 

341  void  constants(); 

342double  Psi(point  pi, point  p2); 

343double  distance(point  pi, point  p2); 

344void  initTangent(); 

345 

346#endif 

347 

348 
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349 


350 

35 1  #ifndef  _MOTOR_H_ 

352#define  _MOTOR_H_ 

353 

354#include  "shepherd.h" 

355 

356/* - * 

357  *  Base  Addressees  for  accessing  Servo  Control  Cards  * 

358  *  Used  in  Home  Testing 

359  * - */ 

360#define  SteerDrivelnteract  .02  /*  used  to  give  stability  to  wheel  */ 
361 

362#define  RadRateTodigit  195.3789  /*  digit/radpersec*/ 

363 

364#define  digitToRadDrive  -6.015495746e-5 


365 

/*  driving  constant  rad/count  =  DPI/104450 

May  8  */ 

366 

/*  Experimental  Results  by  Ed  Mays 

May  7  */ 

367 

/*  Wheel  1  count  =  104456 

*/ 

368 

/*  Wheel  2  count  =  104435 

*/ 

369 

/*  Wheel  3  count  =  104454 

*/ 

370 

/*  Wheel  4  count  =  104455 

*/ 
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371  /*  Average  count  =  104450  */ 

372  /*cf.  2048  *  51  =  104448  */ 

373#define  digitToCmDrive  0.001 1369287 

374  /*  driving  constant  cm/count  =  digitToRadDrive*  18.9cm  5/8/97  */ 

375 

376#define  digitToRadSteer  -6.817692391e-5 

377  /*  steering  constant  rad/count  =  DPI/(2048*45)  19  Apr  */ 

378 

379#define  SteerFBGain  0.000;  /*  steering  feedback  gain  */ 

380#define  DriveFBGain  0.000;  /*  driving  feedback  gain  */ 

381  #define  DigitsHigh  1 023 
382#define  DigitsLow  -1024 

383#define  WheelRadius  18.9  /*  prev  def  in  cm  */ 

384#define  VME9210  0xffff0400  /*  Base  Address  analog  out  to  servo  */ 

385#define  VME9421  OxffffOOOO  /*  Base  address  data  in  from  servo  */ 

386#define  VME2170  OxffffffOO  /*  Base  address  data  out  to  servo  */ 

387#define  VMECTR1  0xffff6000  /*  Counter  */ 

388#define  K5  87.4  /*  control  feedback  constant  (cm/sec)  variable  28  May  ejm  */ 

389#define  DriveFeedBackGain  0.8  /*.8  control  drive  feedback  gain  28  May  ejm  */ 
390#define  angularK3  0.96963  /*  digit/rotational  speed  (rad/sec)  */ 

391#define  steerFeedbackGain  100.0  /*  steering  Feedback  gain  */ 

392#define  angularK5  5.23598 
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393#define  angleFeedbackGain  1000.0 
394 

395extem  unsigned  char  clrLine[6];  /*  ESC-Sequence  for  clear  line  */ 
396extem  char  bwheeldrivecdString[];/*  defined  in  shepherd.c  */ 

397 extern  unsigned  char  bcdString[]; 

398 

399double  Drive_Feedback[ARRAY_SIZE]; 

400 

401  extern  double  desiredAngleRates[], 


402  desiredSpeeds[], 

403  PreviousCountSpeed[], 

404  PreviousCountSteer  [], 

405  DigitToCmDrivef], 

406  Display_Speeds[], 

407  Display_Steers[], 

408  desiredAngles[], 

409  actual  Angles[]; 

410 

411/* - * 

412  *  function  definitions  * 

413  * - */ 


414void  driveS peed(short  []); 
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415void  driveSteer(short  []); 

416void  driveMotors(); 

417void  wheelDrive(void); 

418void  allStop(void); 

419void  updateEncoders(void); 

420 void  updateWheelDrive(void); 

421  void  update WheelSteer( void); 

422 void  displayDirections(void); 

423void  displaySpeed(void); 

424int  readSteerEncoders(unsigned  long  int  []); 

425 void  testDrive(void); 

426void  readEncoders(void); 

427 void  accumulatedriveSpeed(); 

428void  displayDriveAngle();  /*  added  15  may  */ 

429void  drivingFeedback();  /*  28  May  ejm  */ 

430double  velocityReferenceTable(double,int);  /*  28  May  ejm  */ 
43 1  void  steeringFeedback();  /*  4  June  */ 

432double  rateReferenceTable(double);  /*  4  June  */ 

433void  computeActuaIRates();  /*  5  June  */ 

434int  convertDifference(int);  /*  1 1  June  ejm  */ 

435 

436void  align Wheels(void); 
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437 void  clearShaftEncoder(unsigned  short  motors); 

438void  readWheelStatus(unsigned  char  *  array); 

439 

440#endif 

441 

442 

443 

444/*  Timer.h  */ 

445 

446#ifndef  TIMER_H 
447#define  TIMER_H 
448 

449/*  Defines  for  Timer  control  */ 

450#define  TIMER_CTRL  0xfff41002  /*  Control  register  for  Timer  A  */ 

45 1  #define  TIMER_DATA  0xfff41000  /*  Data  register  for  Timer  A  */ 

452 

453 

454/*  settings  master  mode  register  according  to  fig.  1  - 1 2  */ 

455#define  TIMER_MASTER_MODE  OxbafO  /*  timer  master  mode  register  */ 

456  /*  b=BCD  count,  16  Bit  data  bus  */ 

457  /*  4=divide  by  4  */ 

458  /*  8  =  Source  F4  (divide  by  1000)  */ 


231 


459 


/*  0  =  don't  care 


*/ 

460 

461 

462/*  settings  for  counter  mode  register  according  to  fig.  1-17  */ 

463#define  TIMERJMODE  0x0f31  /*  Counter  Mode  Register  Bit  Assignment  */ 


464  /*  0  =  no  gating,  count  on  rising  edge  */ 

465  /*  8  =  Source  F4  (divide  f  by  1000)  */ 

466  /*  3  =  BCD  repetitive  count, reload  load  */ 

467  /*  2  =  count  down,  toggle  TC  */ 

468  /*  or  1  =  count  down,  active  high  Terminal  Count  Pulse  */ 

469 

470/* - * 

47 1  *  function  definitions  * 

472  * - */ 

473 


474  void  TimerHandler(void); 

475  void  timerStart(void); 

476 

477 

478 

479#endif 

480 
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481 


482  Endoftimer.h 


483 


484 

485 

486 

487 

488#ifndef _ MATH_H_ 

489#define _ MATH_H_ 

490 

491 

492 

493double  cos(double  x); 

494double  sin(double  x); 

495double  atan2(double  x,  double  y); 
496double  atan(double  x); 

497double  new_sqrt(double  x); 
498double  new_abs(double  x); 
499double  norm(double  angle); 
500double  min  (double,  double); 
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501  double  max  (double,  double); 

502#endif 

503 

504 

505/* - * 

506*  * 

507  *  File:  SERIAL. H  * 

508  *  * 

509  *  Environment:  GCC  Compiler  v2.7.2  * 

510  *  Last  update:  13  March  1997  * 

5 1 1  *  Name:  Thorsten  Leonardy  * 

5 12  *  Purpose:  Header  File  for  'serial  .c'  * 

513  * - */ 

514 

5 1 5#ifndef  _SERIAL_H_ 

5 1 6#define  _SERIAL_H_ 

517 

518 

5 1 9#define  CONSOLE  0xfff4a000  /*  Base  address  68C68 1  DU  ART  */ 
520 

521/* - * 

522  *  function  definitions  * 
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523  * - */ 

524 

525  void  inPortAHandler(void);  /*  interrupt  handler  */ 

526  void  siolnit(void);  /*  initialize  DUART  */ 

527  void  sioOut(int  hostFlag,  unsigned  char  *s);  /*  Output  a  string  */ 

528  void  gotoXY(int  x,  int  y);  /*  position  cursor  */ 

529 

530#endif 

531 


532/********************************************************************** 


533  End  of  serial.h 


535 

536 
537/* 

538  * 

539  * 
540* 

541  * 

542  * 

543  * 

544  * 


File:  UTILS  .H 

Environment:  GCC  Compiler  v2.7.2 
Last  update:  13  March  1997 
Name:  Thorsten  Leonardy 

Purpose:  Header  File  for  'utils.c' 


* 


* 


* 


* 

* 


* 

* 


* 
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545  * 


*/ 

546 

547#ifndef  _UTILS_H_ 

548#define  _UTILS_H_ 

549 

550/* - * 

55 1  *  function  definitions  * 

552  * - */ 

553 

554 void  setVME(unsigned  char  *board,  unsigned  char  data); 

555 void  toggleVME(unsigned  char  *board,  unsigned  char  data); 

556void  initBoards(void); 

557void  piTest(void); 

558void  piHandler(void); 

559/*void  readClock(void);  */ 

560/*void  WRITE_CLOCK(void);  */ 

561 

562/*  Modified  18  Apr  */ 

563unsigned  char  b2a(int); 

564void  b2a2(unsigned  char  *,  unsigned  char); 

565void  convertToASCII(unsigned  int  ndigits,  unsigned  int  data,  char  *str); 
566void  readJoyStick(void); 
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567 double  insensitive(double  z); 

568 

569#endif 

570 

57 1/***************************************************************** 

572  Endofutils.h 
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APPENDIX  K:  SHEPHERD  OPERATING  MANUAL 
OVERVIEW 

The  Purpose  of  this  document  is  to  provide  a  quick  guide  for  doing  downloads  for  testing 
or  other  purposes.  For  a  more  detailed  guide  see  the  Shepherd  Operators  Guide  (SOG). 

The  Shepherd  compilation  and  download  process  is  a  four  step  process: 

-  Compile  executable  on  workstation. 

-  FTP  S-Records  to  laptop. 

-  Use  Windows  95  HyperTerminal  program  for  direct  connection. 

-  Run  the  program  once  download  complete. 


Compile  Executable  on  Workstation 

1 .  Once  you  have  logged  in  on  the  Shepherd  account,  then  use  the  xinit  command  to 
generate  the  X-Windows  environment. 

2.  In  the  large  terminal  window  type  "cap"  at  the  UNIX  prompt  and  press  the  return  key. 

3.  The  alias  "cap"  logs  you  onto  capella  (the  standard  login  script  will  scroll  by).  The 
Shepherd  group  uses  capella  (server)  because  of  the  nature  of  the  cross  compilation  used 
for  the  "Taurus  board"  and  Motorola  68040  CPU. 

4.  Next,  in  the  large  terminal  window  type  ", taurus "  at  the  prompt  and  press  the  return 
key. 

5.  The  alias  "taurus"  sets  up  the  environment  for  compilation  and  print  services. 

6.  Next,  in  the  large  terminal  window  type  "cd  srk"  at  the  prompt  and  press  the  return 
key;  this  takes  you  to  the  Shepherd  Real-time  Kernel  (~shepherd/srk)  directory.  While  in 
srk  you  can  modify  or  edit  the  require  files  with  your  favorite  editor  (e.g.,  xemacs  or 
nedit).  Once  you  have  completed  your  work,  save  your  files  and  compile.  See  figure  1  on 
the  next  page. 
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r£j  1  "  ■  tnukaol *  *>  Airi/c*  h 

j  shepherd >cap  """h™"" 


..........  tfARNINC  *.*.*•*«.** * 

UNAUTHORIZED  USE  OF  THIS  DEPARTMENT  OF  DEFENSE  (000)  INTEREST 
COH>UTER  SYSTEM  AND/OR  SOFTWARE  IS  PROHIBITED  BY  PUBLIC  LAW. 


This  Is  a  Department  of  Defense  computer  system.  THIS  COMPUTER 
SYSTEM,  INCLUDING  ALL  RELATED  EQUIPMENT,  NETWORKS  AND  NETWORK 
DEVICES  (SPECIFICALLY  INCLUDING  INTERNET  ACCESS),  ARE  PROVIDED 
ONLY  FOR  AUTHORIZED  U.S.  GOVERNMENT  USE.  DOD  computer  systems 
may  be  monitored  for  all  lawful  purposes.  Including  to  ensure 
that  their  use  Is  authorized,  for  management  of  the  system,  to 
facilitate  protection  against  unauthorized  access,  and  to  verify 
security  procedures,  survivability  and  operational  security. 
Monitoring  Includes  active  attacks  by  authorized  DOO  entities 
to  test  or  verify  the  security  of  this  system.  During 
monitoring.  Information  may  be  examined,  recorded,  copied  and 
used  for  authorized  purposes.  All  Information,  Including 

Eersonal  Information,  placed  on  or  sent  over  this  system  may 
e  monitored.  USE  OF  THIS  DOO  COMPUTER  SYSTEM.  AUTHORIZED  OR 
UNAUTHORIZED.  CONSTITUTES  CONSENT  TO  MONITORING  OF  THIS  SYSTEM. 
Evidence  of  unauthorized  use  collected  during  monitoring  may 
be  used  for  administrative,  criminal  or  adverse  action.  USE  OF 
THIS  SYSTEM  CONSTITUTES  CONSENT  TO  MONITORING  FOR  THESE  PURPOSES. 


••****  CLASSIFIED  PROCESSING  ON  THIS  SYSTEM  IS  PROHIBITED  ****•• 
Last  login:  Sat  Jul  19  12:37:58  from  europa.cs.nps.na 


Welcome  to  the  Computer  Science  Department 
Sat  Jul  19  00:30:01  PDT  1997 

You  have  mall, 
shepherd >taurus 
shepherd>cd  srk 
/users/res/shepherd/srk 
shepherd^ 
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Figure  1 :  The  Unix  Workstation  Environment 


7.  Compilation  is  done  through  the  use  of  a  makefile.  Hence,  to  compile  all  you  must  do 
is  type  "make  comp"  at  the  prompt  and  press  the  return  key  (this  will  either  succeed  or 
fail).  If  the  compilation  fails  work  the  errors  provided  by  the  compiler  and  compile  again 
(an  iterative  process).  Once,  the  compilation  is  a  success  you  are  ready  to  FTP  the  S- 
records  to  the  laptop. 


FTP  S-Records  to  Laptop 

8.  To  begin  to  FTP  the  S-records  to  the  laptop  a  few  items  must  be  accomplished.  First, 
the  robot  power  must  be  switched  on  (levers  a,  b,  and  c  on  the  power  supply  in  the  "up" 
or  closed  position  on  the  physical  robot;  provides  power  to  the  robot  and  charges  the 
batteries). 
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See  figure  2  (note  the  "up"  position  below  represents  the  down  position  on  the  physical 
robot). 


Shepherd  Rotary  Vehicle 
Power  Supply 


Figure  2:  Power  Supply  Diagram 


Shepherd 
Rotary  Robot 


9.  Secondly,  the  laptop  must  be  on  and  connected  to  the  local  network  via  the  PCMIA 
card  (ethemet).  Press  the  laptop  "On" button. 

10.  Ensure  the  laptop  powersupply  is  plugged  in,  and  connected  to  the  laptop. 

1 1 .  Ensure  the  therenet  cable  is  properly  connected  to  the  to  the  PC  card. 

12.  After  booting  our  laptop  will  prompt  you  to  login  as  guest—  just  "click  on  the 
cancel"  button.  You  should  see  the  Windows  desk  top  on  the  laptop  (figure  3  below). 
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m 


My  Computer  My  Briefcase 


Network 

Neighborhood 


Inbox 


Shortcut  to 
Telnet 


Shortcut  (3)  to 
Ping 


Recycle  Bin 


The  Internet 


;W$”]FTP95i 

• _ JJEL . j 


The  Microsoft  Shortcut  to 

Ne.worl*.  sei2download 


Start 


1:20. AM 


Figure  3:  Windows  Desk  Top 


13.  The  screen  print  below  describes  the  way  the  windows  should  look.  Now  double 
click  on  the  WS_FTP95  shortcut  to  open  the  ftp  tool. 
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Figure  4:  Windows  WS_FTP95  Shortcut 


14.  At  this  point  the  ftp  tool  opens.  Click  on  the  "OK"  button.  All  information  has  been 
previously  set  for  youfe.g.,  hostname, UserED,  and  Password).  See  figure  5. 

15.  The  next  Window  has  a  split  panel  showing  the  Remote  System  (workstation)  and 
the  Local  System  (the  laptop).  The  directories  have  been  saved,  so  they  always  open  to 
the  correct  directories.  The  file  to  be  ftp'd  is  shepherd.TXT,  it  will  be  ftp'd  from  the 
remote  system  (~shepherd/srk)  to  the  local  system  (c:\shepherdump).  The  file 
shepherd.TXT  contains  the  S-Records  that  will  be  eventually  downloaded  to  the  actual 
robot  CPU.  To  accomplish  the  ftp  highlight  the  file  to  be  transferred  with  your  mouse 
and  click  on  the  arrow  that  points  left  ( See  figure  6).  The  file  transfer  usually  takes 
about  .3  seconds. 
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Figure  5:  Windows  WS_FTP95  Tool 
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16.  The  file  is  now  on  the  "hard"  disk  of  the  laptop.  You  can  now  close  the  window  or 
kill  the  process  by  clicking  on  the  appropriate  button  (active  windowrupper  right  comer 
area  "X”). 
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Use  Windows  95  HyperTerminal  Program  for  Direct  Connection 

17.  You  are  now  back  at  the  Windows  desk  top.  Now  double  click  on  the  ses2download 
shortcut  to  open  a  hard-line,  under  Windows  95  HyperTerminal  (See  figure  7). 
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18.  The  next  window  to  appear  will  be  the  open  HyperTerminal  window  (See  Figure  8). 
Press  the  "reset”  button  on  the  OMNIBYTE,  Taurus  board.  The  Taurus  bug  (debugger) 
prompt  will  appear  in  the  HyperTerminal  window  (See  Figure  8). 


se$2download  -  HyperTerminal 


©e  View  £al  Tranter  Help 


IBB 


Taurus  Debugger  /Diagnostics  Version  3.2  -  11/28/95 

FPC  passed  test 

TaurusJBug>bf  0  40000  0 

Effective  address:  00000000 

Effective  address:  0003FFFC 

Taurus_Bug> 

lo 


t  f* - — n.no.n  . i  wti  nn  iwwou- 


kit  .  i  r  A  DC  iUIIW 


ijB  Start  j  rfpWS_FTP95  LE  131.120.1....||%Ses2download  -  Hype... 


Figure  8:  Windows  HypertTerminal  Window 


19.  Ensure  the  lever  on  the  switch  box  is  placed  on  console  (this  allows  the  console  to 
emulate  a  VT220). 

20.  At  the  Taurus  bug  prompt  type  "bf 0  40000  0"  and  press  the  enter  key.  This 
command  is  called  block  fill  by  the  debugger  it  allows  you  to  disable  the  parity  error 
interrupt  ( PEI)  and  prevents  problems  caused  by  uninitialized  variables.  See  Figure  8. 

21.  At  the  Taurus  bug  prompt  type  "lo"  and  press  the  enter  key.  The  "lo"  command 
initiates  the  download  from  the  console.  See  Figure  8. 


22.  Next  place  the  lever  on  the  switch  box  is  placed  on  host  (this  makes  possible  the  use 
of  the  RS232  protocol  to  download  shephertLTXT  from  c:\shepherdump  to  the  Taurus 
board). 

23.  Click  on  the  HyperTerminal  "Transfer  "option  and  choose  the  "Send  Text  File". 

All  the  "Send  Text  File"  parameters  have  been  previously  set,  so  there  is  no  action  to 
take  in  that  regard. 

See  Figure  9. 
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Figure  9:  Windows  HypertTerminal  Window 


248 


24.  Now  move  to  the  root  directory  and  select  the  c:\shepherdump  directory,  and  double 
click  on  shepherd.TXT  file.  See  figure  10. 


;se$2down!®adl  -  Hype* T  eiimnal 
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shepherd 


Filename:  |  shepherd 
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HEE3 


ri'-sE 


Qpen 


Cancel 


"Icrbnt  i'"  IrAoc^  Uuiu  frt 


1«W  HM  * 


gaSlaitj  ft?  WS_FTP95  LE  131.im...||%»e*2download  -  Hype... 

Figure  10:  Send  File  From  shepherdump 


1:29AM 


The  download  process  is  in  motion.  The  "red"  transmit  light  on  the  RS232  connector  to 
the  switch  box  will  become  faint  while  transmission  is  in  progress.  Once  the 
transmission  is  complete  the  "red"  transmit  light  on  the  RS232  connector  to  the  switch 
box  will  become  a  constant  red;  the  Hyperterminal  window  will  pause  during  the 
transmission  process.  The  Taurus  bug  prompt  will  appear  in  the  HyperTerminal 
window  after  the  transmission  is  complete. 

25.  Ensure  the  lever  on  the  switch  box  is  placed  on  console  (this  allows  the  console  to 
emulate  a  VT220). 
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25.  Now  type  "go  "  at  the  Taurus  bug  prompt  and  press  the  enter  key  (see  figure  11). 
The  program  that  you  have  previously  downloaded  will  be  executed. 


Figure  1 1 :  Taurus  Bug  Prompt  Returns  After  Transmisssion  Completion 


and  the  "go"  Command  is  given  to  Execute  the  Program. 
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After  the  "go"  has  been  given  and  the  execution  begins  the  Shepherd  Main  Menu 
appears  for  your  selection. 


1^  ses2download  -  HyperTerminal 


Be  £cft  View  £a#  Transfer  H# 


HSB 


SHEPHERD  Main  Menu  (Last  Update:  27  Feb  97) 

X - 5 - 0 - S - 0 - S - 0 - S - 0 - s- 


- s - 0 


Please  choose: 


Diagnostics 


(1)  Stop 

(2)  Straight  Motion  (Autonomous) 

(3)  Straight  Motion  by  Joystick 

(4)  XY-Motion  by  Joystick 

(5)  Rotate 

(6)  Sinusoidal  Motion 

(7)  Tornado  (External  Center  of  Rotation) 

(8)  Tornado  (Internal  Center  of  Rotation) 

(9)  Align  Wheels 
(0)  Exit  Program 


j  n.nn.nn  K rri rvx  inmnnM  i  icroni  i  ipaoc  4  mum 


jgBStwtl  frpWS.FTPgLE  131 .120.1. ...11% ^download  -  Hype... 


Figure  12:  Shepherd  Main  Menu 


Note:  remember  this  is  a  quick  guide  and  does  not  provided  answers  to  questions 
concerning  file  size,  debugger  commands,  and  other  requirements  or  constraints. 
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APPENDIX  L:  SENSING  SIMULATION  DATA 


Distance 

X 

y 

theta 

psi 

0.000 

0.400 

0.000 

0.001 

2.355 

0.400 

0.800 

0.001 

0.002 

2.353 

0.800 

1.200 

0.002 

0.005 

2.352 

1.200 

1.600 

0.005 

0.008 

2.350 

1.600 

2.000 

0.008 

0.011 

2.348 

2.000 

2.400 

0.014 

0.015 

2.347 

2.400 

2.800 

0.021 

0.020 

2.345 

2.800 

3.200 

0.029 

0.025 

2.344 

3.200 

3.600 

0.041 

0.031 

2.342 

3.600 

3.999 

0.054 

0.037 

2.341 

4.000 

4.399 

0.070 

0.043 

2.339 

4.400 

4.799 

0.088 

0.050 

2.337 

4.800 

5.198 

0.110 

0.057 

2.336 

5.200 

5.597 

0.134 

0.064 

2.334 

5.600 

5.996 

0.161 

0.072 

2.333 

6.000 

6.395 

0.192 

0.080 

2.331 

6.400 

6.794 

0.225 

0.088 

2.330 

6.800 

7.192 

0.262 

0.096 

2.328 

7.200 

7.590 

0.302 

0.105 

2.326 

7.600 

7.988 

0.346 

0.114 

2.325 

8.000 

8.385 

0.393 

0.123 

2.323 

8.400 

8.782 

0.444 

0.131 

2.322 

8.800 

9.178 

0.498 

0.141 

2.320 

9.200 

9.574 

0.556 

0.150 

2.319 

9.600 

9.969 

0.617 

0.159 

2.317 
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10.000 

10.364 

0.682 

0.168 

2.315 

10.400 

10.758 

0.751 

0.177 

2.314 

10.800 

11.151 

0.823 

0.187 

2.312 

11.200 

11.544 

0.899 

0.196 

2.311 

11.600 

11.936 

0.979 

0.205 

2.309 

12.000 

12.327 

1.062 

0.214 

2.308 

12.400 

12.718 

1.149 

0.224 

2.306 

12.800 

13.107 

1.240 

0.233 

2.304 

13.200 

13.496 

1.334 

0.242 

2.303 

13.600 

13.884 

1.431 

0.251 

2.301 

14.000 

14.271 

1.532 

0.260 

2.300 

14.400 

14.657 

1.637 

0.269 

2.298 

14.800 

15.042 

1.745 

0.278 

2.297 

15.200 

15.426 

1.856 

0.286 

2.295 

15.600 

15.810 

1.971 

0.295 

2.293 

16.000 

16.192 

2.089 

0.303 

2.292 

16.400 

16.573 

2.210 

0.312 

2.290 

16.800 

16.953 

2.334 

0.320 

2.289 

17.200 

17.332 

2.461 

0.328 

2.287 

17.600 

17.711 

2.592 

0.336 

2.286 

18.000 

18.088 

2.725 

0.344 

2.284 

18.400 

18.464 

2.862 

0.352 

2.282 

18.800 

18.839 

3.001 

0.359 

2.281 

19.200 

19.213 

3.143 

0.367 

2.279 

19.600 

19.585 

3.288 

0.374 

2.278 

20.000 

19.957 

3.435 

0.381 

2.276 

20.400 

20.328 

3.586 

0.388 

2.275 

20.800 

20.698 

3.738 

0.395 

2.273 

21.200 

21.066 

3.893 

0.402 

2.271 
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21.600 

21.434 

4.051 

0.408 

2.270 

22.000 

21.801 

4.211 

0.415 

2.268 

22.400 

22.166 

4.373 

0.421 

2.267 

22.800 

22.531 

4.538 

0.427 

2.265 

23.200 

22.894 

4.704 

0.433 

2.264 

23.600 

23.257 

4.873 

0.438 

2.262 

24.000 

23.619 

5.044 

0.444 

2.260 

24.400 

23.980 

5.217 

0.449 

2.259 

24.800 

24.339 

5.391 

0.454 

2.257 

25.200 

24.698 

5.568 

0.459 

2.256 

25.600 

25.056 

5.746 

0.464 

2.254 

26.000 

25.414 

5.926 

0.469 

2.253 

26.400 

25.770 

6.108 

0.474 

2.251 

26.800 

26.126 

6.291 

0.478 

2.249 

27.200 

26.480 

6.476 

0.482 

2.248 

27.600 

26.834 

6.662 

0.486 

2.246 

28.000 

27.188 

6.849 

0.490 

2.245 

28.400 

27.540 

7.038 

0.494 

2.243 

28.800 

27.892 

7.229 

0.497 

2.242 

29.200 

28.243 

7.420 

0.501 

2.240 

29.600 

28.594 

7.613 

0.504 

2.238 

30.000 

28.944 

7.807 

0.507 

2.237 

30.400 

29.293 

8.001 

0.510 

2.235 

30.800 

29.642 

8.197 

0.513 

2.234 

31.200 

29.990 

8.394 

0.516 

2.232 

31.600 

30.338 

8.592 

0.518 

2.231 

32.000 

30.685 

8.790 

0.521 

2.229 

32.400 

31.032 

8.990 

0.523 

2.227 

32.800 

31.378 

9.190 

0.525 

2.226 
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33.200 

31.724 

9.390 

0.527 

2.224 

33.600 

32.070 

9.592 

0.529 

2.223 

34.000 

32.415 

9.794 

0.530 

2.221 

34.400 

32.760 

9.996 

0.532 

2.220 

34.800 

33.105 

10.200 

0.533 

2.218 

35.200 

33.449 

10.403 

0.535 

2.216 

35.600 

33.793 

10.607 

0.536 

2.215 

36.000 

34.137 

10.811 

0.537 

2.213 

36.400 

34.481 

11.016 

0.538 

2.212 

36.800 

34.824 

11.221 

0.538 

2.210 

37.200 

35.167 

11.426 

0.539 

2.209 

37.600 

35.511 

11.632 

0.540 

2.207 

38.000 

35.854 

11.837 

0.540 

2.205 

38.400 

36.197 

12.043 

0.540 

2.204 

38.800 

36.540 

12.249 

0.541 

2.202 

39.200 

36.883 

12.455 

0.541 

2.201 

39.600 

37.225 

12.661 

0.541 

2.199 

40.000 

37.568 

12.867 

0.541 

2.198 

40.400 

37.911 

13.073 

0.541 

2.196 

40.800 

38.254 

13.279 

0.540 

2.194 

41.200 

38.597 

13.484 

0.540 

2.193 

41.600 

38.941 

13.690 

0.539 

2.191 

42.000 

39.284 

13.895 

0.539 

2.190 

42.400 

39.627 

14.100 

0.538 

2.188 

42.800 

39.971 

14.305 

0.537 

2.187 

43.200 

40.315 

14.510 

0.537 

2.185 

43.600 

40.658 

14.714 

0.536 

2.183 

44.000 

41.002 

14.918 

0.535 

2.182 

44.400 

41.347 

15.122 

0.534 

2.180 
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44.800 

41.691 

15.325 

0.532 

2.179 

45.200 

42.036 

15.528 

0.531 

2.177 

45.600 

42.381 

15.730 

0.530 

2.176 

46.000 

42.726 

15.932 

0.528 

2.174 

46.400 

43.072 

16.133 

0.527 

2.172 

46.800 

43.418 

16.334 

0.525 

2.171 

47.200 

43.764 

16.535 

0.524 

2.169 

47.600 

44.111 

16.734 

0.522 

2.168 

48.000 

44.457 

16.934 

0.521 

2.166 

48.400 

44.805 

17.132 

0.519 

2.165 

48.800 

45.152 

17.330 

0.517 

2.163 

49.200 

45.500 

17.528 

0.515 

2.161 

49.600 

45.848 

17.724 

0.513 

2.160 

50.000 

46.197 

17.920 

0.511 

2.158 

50.400 

46.546 

18.116 

0.509 

2.157 

50.800 

46.896 

18.310 

0.507 

2.155 

51.200 

47.246 

18.504 

0.505 

2.154 

51.600 

47.596 

18.697 

0.503 

2.152 

52.000 

47.947 

18.889 

0.500 

2.150 

52.400 

48.298 

19.081 

0.498 

2.149 

52.800 

48.649 

19.272 

0.496 

2.147 

53.200 

49.002 

19.461 

0.493 

2.146 

53.600 

49.354 

19.650 

0.491 

2.144 

54.000 

49.707 

19.839 

0.489 

2.143 

54.400 

50.060 

20.026 

0.486 

2.141 

54.800 

50.414 

20.212 

0.484 

2.139 

55.200 

50.769 

20.398 

0.481 

2.138 

55.600 

51.124 

20.582 

0.478 

2.136 

56.000 

51.479 

20.766 

0.476 

2.135 
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56.400 

51.835 

20.949 

0.473 

2.133 

56.800 

52.191 

21.131 

0.470 

2.132 

57.200 

52.548 

21.311 

0.468 

2.130 

57.600 

52.905 

21.491 

0.465 

2.128 

58.000 

53.263 

21.670 

0.462 

2.127 

58.400 

53.621 

21.848 

0.460 

2.125 

58.800 

53.980 

22.025 

0.457 

2.124 

59.200 

54.339 

22.201 

0.454 

2.122 

59.600 

54.699 

22.376 

0.451 

2.121 

60.000 

55.059 

22.550 

0.448 

2.119 

60.400 

55.420 

22.723 

0.445 

2.117 

60.800 

55.781 

22.894 

0.442 

2.116 

61.200 

56.143 

23.065 

0.440 

2.114 

61.600 

56.505 

23.235 

0.437 

2.113 

62.000 

56.868 

23.403 

0.434 

2.111 

62.400 

57.231 

23.571 

0.431 

2. UO 

62.800 

57.595 

23.738 

0.428 

2.108 

63.200 

57.959 

23.903 

0.425 

2.106 

63.600 

58.323 

24.067 

0.422 

2.105 

64.000 

58.689 

24.231 

0.419 

2.103 

64.400 

59.054 

24.393 

0.416 

2.102 

64.800 

59.420 

24.554 

0.413 

2.100 

65.200 

59.787 

24.714 

0.410 

2.099 

65.600 

60.154 

24.873 

0.407 

2.097 

66.000 

60.522 

25.030 

0.404 

2.095 

66.400 

60.890 

25.187 

0.401 

2.094 

66.800 

61.258 

25.343 

0.398 

2.092 

67.200 

61.627 

25.497 

0.395 

2.091 

67.600 

61.997 

25.650 

0.392 

2.089 
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68.000 

62.367 

25.802 

0.389 

2.088 

68.400 

62.737 

25.953 

0.386 

2.086 

68.800 

63.108 

26.103 

0.383 

2.084 

69.200 

63.479 

26.252 

0.380 

2.083 

69.600 

63.851 

26.400 

0.377 

2.081 

70.000 

64.223 

26.546 

0.374 

2.080 

70.400 

64.596 

26.692 

0.371 

2.078 

70.800 

64.969 

26.836 

0.368 

2.077 

71.200 

65.342 

26.979 

0.365 

2.075 

71.600 

65.716 

27.121 

0.362 

2.073 

72.000 

66.091 

27.262 

0.358 

2.072 

72.400 

66.465 

27.402 

0.355 

2.070 

72.800 

66.841 

27.541 

0.352 

2.069 

73.200 

67.216 

27.678 

0.349 

2.067 

73.600 

67.592 

27.815 

0.347 

2.066 

74.000 

67.969 

27.950 

0.344 

2.064 

74.400 

68.346 

28.084 

0.341 

2.062 

74.800 

68.723 

28.217 

0.338 

2.061 

75.200 

69.100 

28.349 

0.335 

2.059 

75.600 

69.478 

28.480 

0.332 

2.058 

76.000 

69.857 

28.610 

0.329 

2.056 

76.400 

70.236 

28.738 

0.326 

2.055 

76.800 

70.615 

28.866 

0.323 

2.053 

77.200 

70.994 

28.992 

0.320 

2.051 

77.600 

71.374 

29.117 

0.317 

2.050 

78.000 

71.754 

29.241 

0.314 

2.048 

78.400 

72.135 

29.365 

0.311 

2.047 

78.800 

72.516 

29.487 

0.308 

2.045 

79.200 

72.897 

29.607 

0.306 

2.044 
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79.600 

73.279 

29.727 

0.303 

2.042 

80.000 

73.661 

29.846 

0.300 

2.040 

80.400 

74.043 

29.964 

0.297 

2.039 

80.800 

74.426 

30.080 

0.294 

2.037 

81.200 

74.809 

30.196 

0.292 

2.036 

81.600 

75.192 

30.310 

0.289 

2.034 

82.000 

75.576 

30.424 

0.286 

2.033 

82.400 

75.959 

30.536 

0.283 

2.031 

82.800 

76.344 

30.647 

0.281 

2.029 

83.200 

76.728 

30.757 

0.278 

2.028 

83.600 

77.113 

30.867 

0.275 

2.026 

84.000 

77.498 

30.975 

0.272 

2.025 

84.400 

77.883 

31.082 

0.270 

2.023 

84.800 

78.269 

31.188 

0.267 

2.022 

85.200 

78.655 

31.293 

0.264 

2.020 

85.600 

79.041 

31.397 

0.262 

2.018 

86.000 

79.428 

31.500 

0.259 

2.017 

86.400 

79.815 

31.602 

0.257 

2.015 

86.800 

80.202 

31.703 

0.254 

2.014 

87.200 

80.589 

31.803 

0.251 

2.012 

87.600 

80.976 

31.902 

0.249 

2.011 

88.000 

81.364 

32.000 

0.246 

2.009 

88.400 

81.752 

32.097 

0.244 

2.008 

88.800 

82.141 

32.193 

0.241 

2.006 

89.200 

82.529 

32.289 

0.239 

2.004 

89.600 

82.918 

32.383 

0.236 

2.003 

90.000 

83.307 

32.476 

0.234 

2.001 

90.400 

83.696 

32.568 

0.232 

2.000 

90.800 

84.085 

32.659 

0.229 

1.998 
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91.200 

84.475 

32.750 

0.227 

1.997 

91.600 

84.865 

32.839 

0.224 

1.995 

92.000 

85.255 

32.928 

0.222 

1.993 

92.400 

85.645 

33.015 

0.220 

1.992 

92.800 

86.036 

33.102 

0.217 

1.990 

93.200 

86.427 

33.188 

0.215 

1.989 

93.600 

86.817 

33.273 

0.213 

1.987 

94.000 

87.209 

33.357 

0.210 

1.986 

94.400 

87.600 

33.440 

0.208 

1.984 

94.800 

87.991 

33.522 

0.206 

1.982 

95.200 

88.383 

33.603 

0.204 

1.981 

95.600 

88.775 

33.684 

0.201 

1.979 

96.000 

89.167 

33.763 

0.199 

1.978 

96.400 

89.559 

33.842 

0.197 

1.976 

96.800 

89.951 

33.920 

0.195 

1.975 

97.200 

90.344 

33.997 

0.193 

1.973 

97.600 

90.736 

34.073 

0.191 

1.971 

98.000 

91.129 

34.149 

0.189 

1.970 

98.400 

91.522 

34.223 

0.186 

1.968 

98.800 

91.915 

34.297 

0.184 

1.967 

99.200 

92.309 

34.370 

0.182 

1.965 

99.600 

92.702 

34.442 

0.180 

1.964 

100.000 

93.096 

34.513 

0.178 

1.962 

100.400 

93.489 

34.584 

0.176 

1.960 

100.800 

93.883 

34.654 

0.174 

1.959 

101.200 

94.277 

34.723 

0.172 

1.957 

101.600 

94.671 

34.791 

0.170 

1.956 

102.000 

95.066 

34.858 

0.168 

1.954 

102.400 

95.460 

34.925 

0.166 

1.953 
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102.800 

95.855 

34.991 

0.165 

1.951 

103.200 

96.249 

35.056 

0.163 

1.949 

103.600 

96.644 

35.120 

0.161 

1.948 

104.000 

97.039 

35.184 

0.159 

1.946 

104.400 

97.434 

35.247 

0.157 

1.945 

104.800 

97.829 

35.309 

0.155 

1.943 

105.200 

98.224 

35.371 

0.154 

1.942 

105.600 

98.620 

35.432 

0.152 

1.940 

106.000 

99.015 

35.492 

0.150 

1.938 

106.400 

99.411 

35.551 

0.148 

1.937 

106.800 

99.806 

35.610 

0.146 

1.935 

107.200 

100.202 

35.668 

0.145 

1.934 

107.600 

100.598 

35.725 

0.143 

1.932 

108.000 

100.994 

35.782 

0.141 

1.931 

108.400 

101.390 

35.838 

0.140 

1.929 

108.800 

101.786 

35.893 

0.138 

1.927 

109.200 

102.182 

35.948 

0.136 

1.926 

109.600 

102.579 

36.002 

0.135 

1.924 

110.000 

102.975 

36.056 

0.133 

1.923 

110.400 

103.372 

36.108 

0.132 

1.921 

110.800 

103.768 

36.161 

0.130 

1.920 

111.200 

104.165 

36.212 

0.128 

1.918 

111.600 

104.562 

36.263 

0.127 

1.916 

112.000 

104.959 

36.313 

0.125 

1.915 

112.400 

105.355 

36.363 

0.124 

1.913 

112.800 

105.752 

36.412 

0.122 

1.912 

113.200 

106.149 

36.461 

0.121 

1.910 

113.600 

106.547 

36.509 

0.119 

1.909 

114.000 

106.944 

36.556 

0.118 

1.907 
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114.400 

107.341 

36.603 

0.116 

1.905 

114.800 

107.738 

36.649 

0.115 

1.904 

115.200 

108.136 

36.695 

0.114 

1.902 

115.600 

108.533 

36.740 

0.112 

1.901 

116.000 

108.931 

36.784 

0.111 

1.899 

116.400 

109.328 

36.828 

0.109 

1.898 

116.800 

109.726 

36.872 

0.108 

1.896 

117.200 

110.124 

36.915 

0.107 

1.894 

117.600 

110.521 

36.957 

0.105 

1.893 

118.000 

110.919 

36.999 

0.104 

1.891 

118.400 

111.317 

37.040 

0.103 

1.890 

118.800 

111.715 

37.081 

0.102 

1.888 

119.200 

112.113 

37.121 

0.100 

1.887 

119.600 

112.511 

37.161 

0.099 

1.885 

120.000 

112.909 

37.200 

0.098 

1.883 

120.400 

113.307 

37.239 

0.097 

1.882 

120.800 

113.705 

37.277 

0.095 

1.880 

121.200 

114.103 

37.315 

0.094 

1.879 

121.600 

114.502 

37.353 

0.093 

1.877 

122.000 

114.900 

37.389 

0.092 

1.876 

122.400 

115.298 

37.426 

0.091 

1.874 

122.800 

115.697 

37.462 

0.089 

1.872 

123.200 

116.095 

37.497 

0.088 

1.871 

123.600 

116.494 

37.532 

0.087 

1.869 

124.000 

116.892 

37.567 

0.086 

1.868 

124.400 

117.291 

37.601 

0.085 

1.866 

124.800 

117.689 

37.635 

0.084 

1.865 

125.200 

118.088 

37.668 

0.083 

1.863 

125.600 

118.486 

37.701 

0.082 

1.861 
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APPENDIX  M:  SENSING  SIMULATION  CODE  (MAIN2.CC) 


//  File:  main2.cc 
//  Name:  Edward  Mays 
//  Sensing  Simulation 
//Unix 
//GCC 

//  Date:  26  August  1997 
// 

//  Description 

//  THIS  PROGM  SIMULATES  THE  MOVEMENT  OF  A  SQARE  OBJECT  ALONG  A 
//PATH.  THE  OBJECT’S  PATH  DIRECTION  (THETA)  IS  CHANGING,  AS  IS  THE 
//OBJECTS  ORIENTATION  (PSI).  LINE  TRACKING  IS  USED  AND  THE  X-AXIS  IS 
THE  //REFERENCE  LINE.  THE  REFERENCE  LINE  IS  INCREMENTED  BY  40 
UNITS  IN  THE 

// - 

//  Header  file  info 

// - 

#include  <iostream.h> 

#include  <math.h> 

#include  <fstream.h> 

#include  <stdio.h> 


#define  PI  3.14159265358979323846 
#define  RAD  57.2957795 1 308232087684 
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double  deltaTime  =  0.01;//  0.01 

double  Vel  =  40.0; 

double  omega  =  -0.1570796327; 

FILE  *f0,  *fl ,  *f2,  *f3,  *f4,  *f5*f6  ;  //PTR  TO  FILE  FOR  OUTPUT  DATA 


//structure  to  hold  configuration  including  x,  y,  theta,  and  kappa 

typedef  struct  { 
double  x; 
double  y;  } 

POINT; 

typedef  struct  { 

POINT  Point; 
double  Theta; 
double  Kappa; 
double  Psi; 

} 

CONFIGURATION ; 

// - 

//Function:  GetSmooth 
//Return  Value:n/a 
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//Parameters:  function  parm  list 
//Purpose:  gets  users  input  for  sO/smoothness 

// - 

double  GetSmooth(double  &s0) 

{ 


cout « "enter  your  value  for  smoothness  (negatives  not  allowed)"  «endl; 
cin  »  sO; 
return  (sO  >=  0.0); 

}//  GetSmooth 


// - 

//Function:  InitConfig 

//Return  Value:n/a 

//Parameters:  function  parm  list 

//Purpose:  SETS  INITIAL  CONFIGURATION 

// - 

void  InitConfig(CONFIGURATION&  qjnit,  CONFIGURATION* 
q_xax  is, CONFIGURATION  &  qbody, CONFIGURATION*  qfrontR, 

CONFIGURATION*  qfrontL,  CONFIGURATION*  qrearR,  CONFIGURATION* 
qrearL,  CONFIGURATION*  qsnapshot, 

double  &s0,  double  &deltaS) 


{ 

cout«"Setting  the  initial  configuration" 
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«"x=0,  y  =  0,  theta  =  0,  and  kappa  =  0  "«endl; 


q_init.Point.x  =  0.0; 
q_init.Point.y  =  0.0; 
q_init.Theta  =0.0; 
q_init.Kappa  =  0.0; 

q_init.Psi  =  2.356219449;  /*  3*PI/4.0  */ 

cout«"Setting  the  reference  line  configuration" 
«"x  =  0,  y  =  40,  theta  =  0  ,  and  kappa  =  0  "«endl; 

q_xaxis.Point.x  =0.0; 
q_xaxis.Point.y  =40.0; 
q_xaxis.Theta  =  0.0; 
q_xaxis.Kappa  =  0.0; 


//individual  wheels 
qfrontR.Point.x  =  40;  /*  wheel  1  */ 
qfrontR.Point.y  =  -40; 

qfrontL.Point.x  =  40;  /*  wheel2  */ 
qfrontL.Point.y  =  40; 

qrearR.Point.x  =  -40;  /*  wheeB  */ 
qrearR.Point.y  =  -40; 
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qrearL.Point.x  =  -40;  /*  wheel  4  */ 
qrearL.Point.y  =  40; 


qsnapshot.Point.x  =  0.0; 
qsnapshot.Point.y  =  0.0; 

cout«"Enter  size  constant  for  smoothness  <retum>"«endl; 
GetSmooth(sO); 

cout«"Entering  Step  size  constant  deltaS(deltaS=Vel*deltaT)."«endl; 
deltaS  =  Vel*deltaTime;//.05  orig 


}//  InitConfig 


// - 

//Function:  CreateConst 
//Return  Value:n/a 
//Parameters:  function  parm  list 
//Purpose:  create  constants  for 
//  steering  function  dk/ds 
// - 

void  CreateConst(double  &a,  double  &b,  double  &c,  double  &s0) 
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{ 

double  k; 

k  =  1.0/s0;  //all  consts  by  def,  including  curvature 
a  =  3.0*k; 
b  =  3.0*k*k; 
c  =  k*k*k; 

}//  CreateConst 


// - 

//Function:  GetSteerL 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  lambda=dk/ds  (LINEAR  STEERING  FUNCTION) 

// - 

double  GetSteerL(double  &a,  double  &b,  double  &c,  CONFIGURATION&  q, 
CONFIGURATION&  q_xaxis) 

{ 
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double  delta_r; 


delta_r  =  -(q.Point.x  -  q_xaxis.Point.x)*sin(q_xaxis.Theta)  + 
(q.Point.y  -  q_xaxis.Point.y)*cos(q_xaxis.Theta); 

return  (-(a*q.Kappa  +  b*(q.Theta  -  q_xaxis.Theta)  +  c*delta_r)); 
}  //GetSteerL 


// - 

//Function:  GetDeltakappa 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  DETERMINES  THE  KAPPA  DIFFERENCE  PER  INCREMENT  OF  S 
// - 

double  GetDeltakappa(double  &Dk_Ds,  double  &deltaS,  double  &deltakappa) 

{ 

deltakappa  =  Dk_Ds*deltaS; 
retum(deltakappa); 

} //GetDeltakappa 
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// 


//Function:  retumkappa 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  CALCULATES  NEW  VALUE  FOR  KAPPA  USING  deltaK 
// - 

CONFIGURATION  retumkappa(double  &deltakappa,  CONFIGURATION  &q) 

{ 

q.Kappa  =  q.Kappa  +  deltakappa; 
return  q; 

}//retumkappa 


// - 

//Function:  GetS 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  INCREMENTS  S  THROUGH  EACH  ITERATION  OF  THE  WHILE  LOOP 
// - 

double  GetS(double  &s,  double  &deltaS) 

{ 
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s  =  s  +  deltas ; 
return  s; 

}//GetS 

// - 

//Function:  GetDeltaTheta 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  COMPUTES  CHANGE  IN  THETA  PER  INCREMENT  OF  S 
// - 

double  GetDeltaTheta(CONFIGURATION  &q,  double  &deltaS,  double  &deltaT) 

{ 

deltaT  =  q.Kappa*deltaS; 
retum(deltaT); 

}  //GetDeltaTheta 

// - 

//Function:  Circ 
//Return  Value:n/a 
//Parameters:  function  parm  list 
//Purpose:  Circ  function  from  notes  6.29 
// - 
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void  Circ(double  Length,  double  alpha,  CONFIGURATION  &q) 

{ 

double  alpha2,  alpha4; 
alpha2=alpha*alpha; 
alpha4=alpha2*alpha2; 


//configuration  ql 

q.Point.x  =  (1.0  -  alpha2/6.0  +  alpha4/ 1 20.0)  *Length ; 
q.Point.y  =  (0.5  -  alpha2/24.0  +  alpha4/720.0)*Length*alpha; 
q. Theta  =  alpha; 


}//Circ 


// - 

//Function:  Compose 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  updates  the  configuration  and  computes  new  config  (notes  6.2) 

CONFIGURATION  Compose(CONFIGURATION&  q  1  ,CONFIGURATION& 
q2,CONFIGURATION&  q3,  double&  s,double&  deltaTime) 
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{  double  x,y, 
sinTheta  =  sin(q  1  .Theta), 
cosTheta  =  cos(ql. Theta); 

x  =  ql.Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*  sinTheta; 
y  =  ql.Point.y  +  q2.Point.x*sinTheta  +  q2.Point.y*cosTheta; 
q3. Point. x  =  x; 
q3.Point.y  =  y; 

q3.Theta  =  ql. Theta  +  q2.Theta; 

q3.Psi  =  ql .Psi  +  (omega  *  deltaTime);  /*  how  to  handle  move  left/right?  */ 

fprintf(f6,"%  10.3f  %10.3f  %10.3f  %10.3f  %10.3f\n", 
s,q3.Point.x,  q3.Point.y,q3.Theta,  q3.Psi); 
return  q3; 

}//  end  Compose 


CONFIGURATION  Compose2(CONFIGURATION&  q  1  .CONFIGURATION*  q2, 
CONFIGURATION  &  q3)  /^position  */ 


{  double  x,y, 
sinTheta  =  sin(ql.Psi), 
cosTheta  =  cos(q  1  .Psi); 
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x  =  ql.Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*sinTheta; 
y  =  ql.Point.y  +  q2.Point.x*sinTheta  +  q2.Point.y*cosTheta; 

q3.Point.x  =  x; 
q3.Point.y  =  y; 
return  q3; 

}//  end  Compose2 


// - 

//Function:  Openfile 
//Return  Value:n/a 
//Parameters:  function  parm  list 
//Purpose:  To  compute  transposition 

// - 

void  Openfile() 

{ 

fO  =  fopen("drk.dat","w"); 
fl  =fopen("wheell.dat","w"); 
f2  =  fopen("wheel2.dat", "w"); 
f3  =  fopen("wheel3.dat","w"); 
f4  =  fopen("wheel4.dat","w"); 
f5  =  fopen("composite.dat", ”w"); 
f6  =  fopen("psi.dat","w”); 

}//  Openfile 
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// 


//Function:  Print 
//Return  Value:n/a 
//Parameters:  function  parm  list 
//Purpose:  To  compute  transposition 

// - — 

void  printFile(FILE  *f, CONFIGURATION  &q) 

{ 

fprintf(f,"%10.3f  %10.3f\n", 
q.Point.x,  q.Point.y); 


}//  printFile 


// - 

//Function:  blankLine 
//Return  Value:n/a 
//Parameters:  function  parm  list 
//Purpose:  To  compute  transposition 

void  blankLine(FILE  *0 

{ 
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fprintf(f,"\n"); 


}//  blankLine 


// - 

//Function:  updateWheels 
//Return  Value:n/a 
//Parameters:  function  parm  list 
/Purpose:  create  constants  for 
//  steering  function  dk/ds 
// - 

void  updateWheel  s(CONFIGURATION  &  qbody,  CONFIGURATION*  qfrontR, 
CONFIGURATION  &  qfrontL,  CONFIGURATION  qrearR,  CONFIGURATION  qrearL, 
CONFIGURATION*  qwheell,  CONFIGURATION*  qwheel2,  CONFIGURATION  & 
qwheeB,  CONFIGURATION*  qwhee!4,  CONFIGURATION  &  q3,int&  s2) 

{  printFile(fO, qbody); 

qwheell  =  Compose2(qbody,qfrontR,q3); 

printFile(f  1  ,qwheel  1 ); 

blankLine(fl); 

qwheel2  =  Compose2(qbody,qfrontL,q3); 
printFile(f2,qwheel2); 
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blankLine(f2); 

qwheel3  =  Compose2(qbody,  qrearR,  q3); 

printFile(f3,qwheel3); 

blankLine(f3); 

qwheel4  =  Compose2(qbody,  qrearL,q3); 
printFile(f4,qwheel4) ; 
blankLine(f4); 

if((s2=0.0)ll(s2%  100=0))  { 

printFile(f5,qwheel  1 ); 
printFile(f5  ,qwheel2) ; 
printFile(f5  ,qwheel4) ; 
printFile(f5,qwheel3); 
printFile(f5  ,qwheel  1 ) ; 
blankLine(f5); 

} 

}//  update  Wheels 


int  main() 


{  CONFIGURATION  q,  q_xaxis,  New_q,  qbody,  qfrontR,  qfrontL,  qrearR,  qrearL, 
qwheell,  qwheel2,  qwheeB,  qwheel4,  qsnapshot,q3; 

int  ix,s2, counter; 
double  a,  b,  c; 
double  s,sO; 
double  Dk_Ds; 
double  deltaS ; 
double  deltaT; 
double  deltaK; 
const  double  Sdig  =  0.001; 
double  smax=400.0; 

InitConfig(q,  q_xaxis,  qbody,  qfrontR,  qfrontL,  qrearR,  qrearL,qsnapshot,sO,  deltaS); 
//configuaration  set  up 

Openfile(); 

printFile(fO,q); 

printFile(fl, qfrontR);  //write  initial  config  to  file 

printFile(f2,  qfrontL); 
printFile(f3,qrearR); 
printFile(f4,qrearL); 

//printFile(f5,qfrontR); 

//printFile(f5,qfrontL); 


//constants  equation  6.3 

//s0  is  smoothness,  s  is  the  incremental  step 


//const  used  for  prec/toler 
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//printFile(f5  ,qrearL) ; 

//printFile(f5  ,qrearR) ; 

printFile(f5  ,qfrontR) ; 
blankLine(f5); 

CreateConst(a,  b,  c,  sO);  //calcs  consts 

s  =  0.0; 

s2=s; 

counter=0; 

for(ix=0;  ix<10;ix++){ 
do 
{// 

Dk_Ds  =  GetSteerL(a,  b,  c,  q,  q_xaxis);  //calculates  lambda  =dk/ds 
deltaK  =  GetDeltakappa(Dk_Ds,  deltas,  deltaK);  //lambda*deltaS 
retumkappa(deltaK,  q);  //Kappa  <=  kappa  +  deltaK 

deltaT  =  GetDeltaTheta(q,  deltas,  deltaT);  //Theta  <=  Theta  +  deltaT 
Circ(deltaS,  deltaT,  New_q);  //cir 

qbody  =  Compose(q,  New_q,  q,  s,  deltaTime);  //compose 

update  Wheels(qbody,qfrontR,  qfrontL,  qrearR, 

qrearL,qwheel  1 ,  qwheel2,  qwheeI3, 
qwheel4,q3,s2); 

GetS(s,  deltaS); 
s2=s; 
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}  while  (s<smax); 

//  } while  ((fabs(q.Point.y)  >  Sdig)ll(fabs(q.Theta)  >  Sdig)  II 

//  (fabs(q.Kappa)  >  Sdig)); 

s=0.0; 

s2=s; 

q_xaxis.Point.y  =  q_xaxis.Point.y  +  40.0; 

if(ix%2=0){ 
q_xaxis. Theta  =  PI; 
q.Theta  =  PI; 
omega  =  fabs(omega); 

}else{ 

q_xaxis.Theta  =  0.0; 
q.Theta  =  0.0; 
omega  =  -omega; 

} 

} 

fclose(fO); 
fclose(fl); 
fclose(f2); 
fclose(f3); 
fclose(f4); 
fclose(f5); 
fclose(f6); 
return  0; 

}//end  main2.cc 
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APPENDIX  N:  INPUT  VS.  OUPUT  VELOCITY 


-1024  -87.657 
-1023  -87.429 
-1020  -86.975 
-1015  -86.747 
-1010-86.406 
-1005  -85.838 
-1000-85.383 
-900  -76.856 
-800  -67.998 
-700  -59.082 
-600-51.048 
-500  -42.748 
-400  -34.107 
-375  -31.834 
-350  -29.673 
-300  -25.580 
-250  -21.260 
-225-19.100 
-200-17.053 
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-175  -14.780 


-150  -12.506 
-125  -10.459 
-100-8.413 
-90  -7.503 
-80  -6.707 
-70  -5.798 
-60  -5.002 
-50  -4.092 
-40  -3.297 
-30  -2.387 
-20-1.591 
-10  -.682 
-5  -.341 
-1  -.113 
0.0  0.0 
1023  87.429 
1020  86.975 
1015  86.747 
1010  86.406 
1005  85.838 
1000  85.383 
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900  76.856 


800  67.998 
700  59.082 
600  51.048 
50042.748 
400  34.107 
375  31.834 
350  29.673 
300  25.580 
250  21.260 
225  19.100 
200  17.053 
175  14.780 
150  12.506 
125  10.459 
100  8.413 
90  7.503 
80  6.707 
70  5.798 
60  5.002 
504.092 
40  3.297 
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30  2.387 


20  1.591 
10  .682 
5.341 
1  .113 


286 


APPENDIX  O:  INPUT  VS  OUTPUT  STEERING  RATES 


A.  DESIRED  INPUT  RATE  VS  ACTUAL  (BOTH  ESTIMATED  AND 
SOFTWARE  DEPENDENT) 


Desired  rate  of 

turn 

Time 

Stopwatch 

(sec) 

Estimated  Rate 

(rad/s) 

Software 

Measured  Rate 

(rad/s,  average) 

1 

6.0 

1.00000 

0.98174 

2 

3.5 

1.79485 

1.95667 

3 

2.19 

2.86849 

2.93160 

5 

1.69 

2.71716 

3.90653 

5.5 

No  data 

No  data 

4.88828 

10 

No  data 

No  data 

5.23598 

20 

No  data 

No  data 

5.23598 

30 

No  data 

No  data 

5.23598 

Figure  A.1  Inputs  and  results  from  massaged  data  (error).  No  data  entries  exist  because  the 
revolutions  were  too  fast  for  hand  timing. 


B.  DESIRED  INPUT  RATE  VS  OUPUT  FOR  EACH  WHEEL  (SOFTWARE 
DEPENDENT 

Below  M5,  M6,  M7,  and  M8  reperesent  the  steering  motors  for  wheel  1,  wheel  2, 
wheel3,  and  wheel4  respectively. 


Desired  Rate 

of  turn 

M5  Rate 

M6  Rate 

M7  Rate 

M8  Rate 

1 

1.002 

.9975 

.9965 

.999 

2 

2.0025 

1.997 

■Hil 

1.997 

3 

3.005 

2.995 

2.9975 

3.005 

4 

4.004 

3.996 

3.9925 

5 

5.008 

5.002 

4.9985 

5.002 

Desired  Rate 
of  turn 


MS  Rate 

M6  Rate 

M7  Rate 

M8  Rate 

5.1035 

5.0935 

5.093 

5.0955 

5.2065 

5.198 

5.1955 

5.198 

5.238 

5.235 

5.234 

5.235 

5.238 

5.235 

5.234 

5.235 

-1.001 

-1.002 

-1.002 

-1.002 

-2.001 

-2.003 

-2.003 

-2.004 

-3.0055 

-3.006 

-3.0055 

-3.006 

-4.007 

-4.006 

-4.0015 

-4.008 

-5.010 

-5.010 

-5.009 

-5.010 

-5.107 

-5.110 

-5.1035 

-5.112 

-5.2105 

-5.2125 

-5.2075 

-5.214 

-5.238 

-5.2415 

-5.237 

-5.2535 

-5.238 

-5.2415 

-5.237 

-5.2535 

Figure  A.2:  Desired  (commanded)  rate  of  turn  vs.  actual  “free  floating”  motor  rate. 


C.  DESIRED  INPUT  (DIGIT  MANIPULATION)  VS  OUTPUT  RATE 
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Input 

M5  Rate 

M6Rate 

M7  Rate 

M8  Rate 

(digits) 

(rad/s) 

(rad/s) 

(rad/s) 

(rad/s) 

300 

1.539 

1.533 

1.533 

1.533 

400 

2.049 

2.046 

2.045 

2.045 

500 

2.561 

2.556 

2.556 

2.556 

600 

3.074 

3.068 

3.067 

3.068 

700 

3.584 

3.579 

3.579 

3.579 

800 

4.097 

4.092 

4.091 

4.092 

900 

4.610 

4.602 

4.602 

4.602 

1000 

5.124 

5.116 

5.114 

5.116 

1010 

5.174 

5.172 

5.170 

5.172 

1020 

5.226 

5.218 

5.216 

5.218 

1021 

5.233 

5.226 

5.222 

5.225 

1022 

5.235 

5.233 

5.231 

5.232 

1023 

5.237 

5.235 

5.234 

5.235 

Figure  A.3:  Desired  (commanded)  rate  of  turn  vs.  actual 4 ‘free  floating”  motor  rate  for  each  wheel 
using  input  digits . 


WHEEL  4  ROTATION  DATA 


Clockwise  Rotation 


000.867 


000.878 


000.976 


000.933 


000.984 


000.992 


000.992 


000.902 


Counterclockwise 

Rotation 


360.390 


360.390 


360.363 


360.414 


360.371 


360.394 


360.453 


360.394 


9 

000.' 

S>96 

360.445 

10 

000.! 

996 

360.476 

11 

001.003 

360.402 
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Clockwise  Rotation 

Counterclockwise 

Rotation 

12 

000.917 

360.417 

13 

000.996 

360.433 

14 

001.003 

360.468 

15 

000.972 

360.468 

16 

000.863 

360.472 

17 

000.968 

360.398 

18 

000.941 

360.425 

19 

000.957 

360.480 

20 

000.937 

360.480 

Figure  A.4:  Wheel  4  data  based  on  position  of  rest  after  direction  of  turn. 
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